From 1054f54aed07ef1846c763b787007d215e3d15f4 Mon Sep 17 00:00:00 2001 From: K0-p Date: Thu, 29 Aug 2024 17:45:25 -0500 Subject: [PATCH 1/5] updated README.md --- README.md | 23 ++++++++++++++++++----- UI/foxglove/UI_launcher.py | 5 ++++- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 89f53487..14e7ac77 100644 --- a/README.md +++ b/README.md @@ -7,18 +7,23 @@ rpc is a software framework designed for generating task trajectories (planning) Software Framework is developed by Seung Hyeon Bang based on the [PnC](https://github.com/junhyeokahn/PnC) Repository.
## Dependencies -The controller has been tested on Ubuntu 18.04, Ubuntu 20.04, and Mac OSX Ventura. It builds on the shoulders of the following software:
+The controller has been tested on Ubuntu 18.04, Ubuntu 20.04, Ubuntu 23.04, and Mac OSX Ventura. It builds on the shoulders of the following software:
- [anaconda](https://docs.anaconda.com/anaconda/install/): For Pybullet simulator
- python dependencies: ``` $ conda env create -f rpc.yml ``` +- [conan](https://github.com/conan-io/conan): package manager for C/C++ +``` +$ pip install conan +``` - [pinocchio](https://github.com/shbang91/pinocchio): rigid body dynamics ###### optional dependencies - [MatLogger2](https://github.com/shbang91/MatLogger2): logging numeric data (cpp to MAT-files) - [zmq](https://github.com/shbang91/rpc/blob/main/dependency/scripts/install_zmq.sh): logging numeric data - [protobuf](https://github.com/shbang91/rpc/blob/main/dependency/scripts/install_protobuf.sh): logging numeric data +- [Foxglove](https://github.com/foxglove): websocket & schema protocols for robot visualization and parameter operations ## Usage - Source conda environment:
@@ -27,7 +32,7 @@ $ conda activate rpc ``` - Compile:
``` -$ mkdir build +$ conan install conanfile.txt --build=missing $ cd build $ cmake .. $ make -j4 @@ -36,12 +41,20 @@ $ make -j4 ``` python simulator/pybullet/draco_main.py ``` -###### optional -- visualization:
+###### Foxglove (optional) +- Project should be built with the following flags: +``` +BUILD_WITH_FOXGLOVE ON +BUILD_WITH_ZMQ_PROTOBUF ON +``` +- Run Foxglove:
``` $ conda env create -f visualize.yml $ conda activate visualize -$ python plot/draco/draco_data_manager.py --b_visualize=true +$ python UI/foxglove/UI_launcher.py --visualizer=foxglove ``` +- Access Foxglove server via either:
+ 1) [Foxglove webservice](https://app.foxglove.dev/) + 2) [Foxglove application](https://foxglove.dev/download) ###### Hardware Usage - Please refer to this [repository](https://github.com/shbang91/draco3_nodelet) using rpc library diff --git a/UI/foxglove/UI_launcher.py b/UI/foxglove/UI_launcher.py index 85ffb77c..4a66ad42 100644 --- a/UI/foxglove/UI_launcher.py +++ b/UI/foxglove/UI_launcher.py @@ -211,7 +211,10 @@ async def main(): color[lf] = [.1, .5, 1, 1] proj_foot_pos[rf], proj_foot_pos[lf] = [0, 0, 0], [0, 0.184, 0] proj_foot_ori[rf], proj_foot_ori[lf] = [0, 0, 0, 0], [0, 0, 0, 0] - name = "projected_footsteps_" + str(i) + if i < 10: + name = "projected_footsteps_0" + str(i) + else: name = "projected_footsteps_" + str(i) + # Add a channel for projected footstep data foot_scene = await SceneChannel( True, name, "json", name, ["rf_pos_x","rf_pos_y","rf_pos_z","rf_ori_x","rf_ori_y","rf_ori_z","rf_ori_q", From e7fc4d7c7dd7115f1d6e2e13b581265629bd03b2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 2 Sep 2024 21:42:48 +0000 Subject: [PATCH 2/5] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.5.5 → v0.6.3](https://github.com/astral-sh/ruff-pre-commit/compare/v0.5.5...v0.6.3) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index bc4a0088..72525ddb 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: - id: clang-format types_or: [c++] - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.5.5 + rev: v0.6.3 hooks: - id: ruff # ignore import-star and import-star-usage From 6a46948adb031eb6f07f80f56e39b678e9739486 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 2 Sep 2024 21:43:06 +0000 Subject: [PATCH 3/5] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- UI/convert_pkl2mcap.py | 48 +- UI/foxglove/UI_launcher.py | 179 +- UI/foxglove/client/parameter_subscriber.cpp | 6 +- UI/foxglove/footstep_planner.py | 53 +- UI/visualization_toolbox.py | 17 +- config/draco/hw/ihwbc/MPC_LOCOMOTION.yaml | 1 - config/draco/hw/ihwbc/pnc.yaml | 6 +- config/draco/hw/wbic/MPC_LOCOMOTION.yaml | 1 - config/draco/hw/wbic/nodelet.yaml | 1 - config/draco/hw/wbic/pnc.yaml | 8 +- .../sim/mujoco/ihwbc/MPC_LOCOMOTION.yaml | 1 - .../draco/sim/mujoco/ihwbc/mujoco_params.yaml | 11 +- config/draco/sim/mujoco/ihwbc/pnc.yaml | 8 +- .../draco/sim/mujoco/wbic/MPC_LOCOMOTION.yaml | 1 - .../draco/sim/mujoco/wbic/mujoco_params.yaml | 11 +- config/draco/sim/mujoco/wbic/pnc.yaml | 10 +- .../sim/pybullet/ihwbc/MPC_LOCOMOTION.yaml | 1 - config/draco/sim/pybullet/ihwbc/pnc.yaml | 8 +- .../sim/pybullet/ihwbc/pybullet_params.py | 71 +- .../sim/pybullet/wbic/MPC_LOCOMOTION.yaml | 1 - config/draco/sim/pybullet/wbic/pnc.yaml | 10 +- .../sim/pybullet/wbic/pybullet_params.py | 71 +- controller/control_architecture.hpp | 2 +- .../draco_crbi/draco3_crbi_helper.h | 30 +- .../draco_controller/draco_data_manager.hpp | 2 +- .../draco_controller/draco_interface.cpp | 7 +- .../draco_controller/draco_interface.hpp | 4 +- .../draco_kf_state_estimator.cpp | 2 +- .../draco_kf_state_estimator.hpp | 2 +- .../draco_task/draco_com_xy_task.cpp | 4 +- .../draco_task/draco_wbo_task_helper.cpp | 7653 +++++++++++------ .../draco_task/draco_wbo_task_helper.h | 30 +- .../draco_controller/state_estimator.hpp | 2 +- controller/interface.hpp | 2 +- mujoco/CMakeLists.txt | 1 - mujoco/include/mujoco/mjdata.h | 600 +- mujoco/include/mujoco/mjexport.h | 44 +- mujoco/include/mujoco/mjmacro.h | 30 +- mujoco/include/mujoco/mjplugin.h | 173 +- mujoco/include/mujoco/mjrender.h | 202 +- mujoco/include/mujoco/mjthread.h | 24 +- mujoco/include/mujoco/mjtnum.h | 25 +- mujoco/include/mujoco/mjvisualize.h | 1011 ++- mujoco/include/mujoco/mjxmacro.h | 1382 ++- mujoco/include/mujoco/mujoco.h | 1181 +-- planner/locomotion/convex_mpc/CMakeLists.txt | 10 +- .../include/convex_mpc/mpc_solution.hpp | 4 +- .../locomotion/convex_mpc/test/CMakeLists.txt | 10 +- .../convex_mpc/test/cmake/gtest.cmake | 4 +- plot/convex_mpc.m | 14 +- plot/draco/draco_data_manager_mpc.py | 180 +- plot/draco/experiment_replay.py | 80 +- plot/foxglove_utils.py | 4 - plot/meshcat_utils.py | 54 +- scripts/draco_manipulation_comm.py | 62 +- scripts/real_teleop.py | 264 +- .../mujoco/draco/simulate/array_safety.h | 25 +- .../mujoco/draco/simulate/glfw_adapter.cc | 24 +- .../mujoco/draco/simulate/glfw_dispatch.h | 6 +- simulator/mujoco/draco/simulate/lodepng.cpp | 2 +- simulator/mujoco/draco/simulate/lodepng.h | 1299 +-- .../draco/simulate/platform_ui_adapter.h | 36 +- simulator/pybullet/draco_main.py | 304 +- simulator/pybullet/draco_main_wbic.py | 1106 +-- simulator/pybullet/draco_manipulation_main.py | 569 +- simulator/pybullet/fixed_draco_main.py | 88 +- simulator/pybullet/go2_main.py | 14 +- test/dcm_planner_test.cpp | 3 +- test/karnopp_compensator_test.cpp | 2 +- util/python_utils/comm.py | 52 +- util/python_utils/demo.py | 21 +- util/python_utils/device/base.py | 28 +- util/python_utils/device/t265.py | 55 +- util/python_utils/pybullet_util.py | 132 +- 74 files changed, 10156 insertions(+), 7233 deletions(-) diff --git a/UI/convert_pkl2mcap.py b/UI/convert_pkl2mcap.py index 2f6a9375..a001caba 100644 --- a/UI/convert_pkl2mcap.py +++ b/UI/convert_pkl2mcap.py @@ -3,6 +3,7 @@ This assumes a specific set of parameters available in the pkl file, such as time, base_pos, base_ori, joint_positions, icp_est, icp_des, etc. """ + import os import sys import argparse @@ -44,7 +45,7 @@ def main(): b_using_kf_estimator = args.b_using_kf_estimator # variables in pkl file - time =[] + time = [] base_pos, base_ori, joint_positions = [], [], [] vis_2d_object_names = ["icp_est", "icp_des"] vis_3d_object_names = ["lf_pos", "rf_pos"] @@ -97,7 +98,9 @@ def main(): icp_des_scene = create_sphere_scene("icp_des", [0.0, 1.0, 0.0, 0.5]) # send data to mcap file - with open(cwd + "/experiment_data/draco3_foxglove.mcap", "wb") as f, Writer(f) as mcap_writer: + with open(cwd + "/experiment_data/draco3_foxglove.mcap", "wb") as f, Writer( + f + ) as mcap_writer: for i in range(len(time)): # Update all transforms (to visualize URDF) vis_q[0:3] = np.array(base_pos[i]) @@ -109,41 +112,50 @@ def main(): pin.updateGeometryPlacements(model, data, visual_model, visual_data) for visual in visual_model.geometryObjects: update_robot_transform(visual, visual_data, visual_model, transform) - mcap_writer.write_message("transforms", transform, int(time[i] * 1e9), int(time[i] * 1e9)) + mcap_writer.write_message( + "transforms", transform, int(time[i] * 1e9), int(time[i] * 1e9) + ) transform.rotation.Clear() transform.translation.Clear() # ------------------------------------------- # update transform of ADDITIONAL visual elements for vname, vval in vis_2d_dict.items(): update_2d_transform(vname, vval[i], transform) - mcap_writer.write_message( "transforms", transform, - int(time[i] * 1e9), int(time[i] * 1e9)) + mcap_writer.write_message( + "transforms", transform, int(time[i] * 1e9), int(time[i] * 1e9) + ) transform.rotation.Clear() transform.translation.Clear() # =========================================== # ICP visuals (est / des) icp_est_scene.entities[0].timestamp.FromNanoseconds(int(time[i] * 1e9)) - mcap_writer.write_message( "icp_est_viz", icp_est_scene, - int(time[i] * 1e9), int(time[i] * 1e9)) + mcap_writer.write_message( + "icp_est_viz", icp_est_scene, int(time[i] * 1e9), int(time[i] * 1e9) + ) # ------------------------------------------- icp_des_scene.entities[0].timestamp.FromNanoseconds(int(time[i] * 1e9)) - mcap_writer.write_message( "icp_des_viz", icp_des_scene, - int(time[i] * 1e9), int(time[i] * 1e9)) + mcap_writer.write_message( + "icp_des_viz", icp_des_scene, int(time[i] * 1e9), int(time[i] * 1e9) + ) # =========================================== # ICP plots (est / des) for vname, vval in vis_2d_dict.items(): - mcap_writer.write_message(vname, - Point2(x=vval[i][0], y=vval[i][1]), - int(time[i] * 1e9), - int(time[i] * 1e9)) + mcap_writer.write_message( + vname, + Point2(x=vval[i][0], y=vval[i][1]), + int(time[i] * 1e9), + int(time[i] * 1e9), + ) # Feet position plots (left / right) for vname, vval in vis_3d_dict.items(): - mcap_writer.write_message(vname, - Point3(x=vval[i][0], y=vval[i][1], z=vval[i][2]), - int(time[i] * 1e9), - int(time[i] * 1e9)) + mcap_writer.write_message( + vname, + Point3(x=vval[i][0], y=vval[i][1], z=vval[i][2]), + int(time[i] * 1e9), + int(time[i] * 1e9), + ) mcap_writer.finish() if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/UI/foxglove/UI_launcher.py b/UI/foxglove/UI_launcher.py index 4a66ad42..b69fe2f7 100644 --- a/UI/foxglove/UI_launcher.py +++ b/UI/foxglove/UI_launcher.py @@ -9,7 +9,7 @@ sys.path.append(cwd) sys.path.append(cwd + "/build") -#NEED TO GET THIS BACK --- FIX LATER +# NEED TO GET THIS BACK --- FIX LATER from util.python_utils.util import rot_to_quat, quat_to_rot from messages.draco_pb2 import * @@ -25,9 +25,9 @@ parser.add_argument( "--visualizer", choices=["none", "meshcat", "foxglove"], default="none" ) -parser.add_argument("--robot", - choices=["draco", "fixe_draco", "manipulator"], - default="draco") +parser.add_argument( + "--robot", choices=["draco", "fixe_draco", "manipulator"], default="draco" +) parser.add_argument("--hw_or_sim", choices=["hw", "sim"], default="sim") args = parser.parse_args() @@ -52,10 +52,7 @@ from watchdog.observers import Observer # local tools to manage Foxglove scenes - from plot.foxglove_utils import ( - SceneChannel, - ShapeScene - ) + from plot.foxglove_utils import SceneChannel, ShapeScene from UI.visualization_toolbox import update_robot_transform # load parameters that can be controlled / changed and start Control Parameters server @@ -65,7 +62,7 @@ target=asyncio.run, args=([foxglove_ctrl.run(step_listener)]) ) th_slow.start() - time.sleep(20.) # give some time for state estimator to publish data + time.sleep(20.0) # give some time for state estimator to publish data scene_schema = b64encode( build_file_descriptor_set(SceneUpdate).SerializeToString() @@ -94,11 +91,12 @@ except yaml.YAMLError as exc: print(exc) -pnc_path = ("config/" + args.robot + "/" + - args.hw_or_sim + "/" + env + "/" + wbc + "/pnc.yaml") +pnc_path = ( + "config/" + args.robot + "/" + args.hw_or_sim + "/" + env + "/" + wbc + "/pnc.yaml" +) grf_names = ["lfoot_rf_cmd", "rfoot_rf_cmd"] - # "lfoot_rf_normal_filt", "rfoot_rf_normal_filt"] +# "lfoot_rf_normal_filt", "rfoot_rf_normal_filt"] xyz_scene_names = [ "torso_ori_weight", @@ -111,6 +109,7 @@ ] xyz_scenes = [] + def foot_dimensions(): with open(pnc_path, "r") as pnc_stream: try: @@ -121,6 +120,7 @@ def foot_dimensions(): print(exc) return foot_half_length, foot_half_width + async def sceneinitman(name, server): x = await SceneChannel(True, name, "json", name, ["x", "y", "z"]).add_chan(server) xyz_scenes.append([x, name]) @@ -174,72 +174,103 @@ async def main(): True, "icp_des", "json", "icp_des", ["x", "y"] ).add_chan(server) proj_footstepS_chan_id = await SceneChannel( - False, "proj_footstep_viz", "protobuf", SceneUpdate.DESCRIPTOR.full_name, scene_schema + False, + "proj_footstep_viz", + "protobuf", + SceneUpdate.DESCRIPTOR.full_name, + scene_schema, ).add_chan(server) b_ft_contact_chan_id = await SceneChannel( True, "b_ft_contact", "json", "b_ft_contact", ["b_lfoot", "b_rfoot"] ).add_chan(server) lf_pos_chan_id = await SceneChannel( - True, "lf_pos", "json", "lf_pos", - ["x", "y", "z"] + True, "lf_pos", "json", "lf_pos", ["x", "y", "z"] ).add_chan(server) rf_pos_chan_id = await SceneChannel( - True, "rf_pos", "json", "rf_pos", - ["x", "y", "z"] + True, "rf_pos", "json", "rf_pos", ["x", "y", "z"] ).add_chan(server) - for scn in range(len(xyz_scene_names)): await sceneinitman(xyz_scene_names[scn], server) # STEP_MAX available footstep plans hfoot_length, hfoot_width = foot_dimensions() - x = 2*hfoot_length - y = 2*hfoot_width + x = 2 * hfoot_length + y = 2 * hfoot_width msgs, size, color = [], {}, {} proj_footstep_chan_ids = [] proj_footstep_viz_chan_ids = [] - proj_foot_pos, proj_foot_ori = {}, {} # Stores step position + proj_foot_pos, proj_foot_ori = {}, {} # Stores step position proj_feet = [] for i in range(STEP_MAX): - rf = "proj_rf"+str(i) - lf = "proj_lf"+str(i) + rf = "proj_rf" + str(i) + lf = "proj_lf" + str(i) msgs.append(rf) msgs.append(lf) size[rf], size[lf] = [x, y, 0.001], [x, y, 0.001] color[rf] = [1, 0, 0, 1] - color[lf] = [.1, .5, 1, 1] + color[lf] = [0.1, 0.5, 1, 1] proj_foot_pos[rf], proj_foot_pos[lf] = [0, 0, 0], [0, 0.184, 0] proj_foot_ori[rf], proj_foot_ori[lf] = [0, 0, 0, 0], [0, 0, 0, 0] if i < 10: name = "projected_footsteps_0" + str(i) - else: name = "projected_footsteps_" + str(i) + else: + name = "projected_footsteps_" + str(i) # Add a channel for projected footstep data foot_scene = await SceneChannel( - True, name, "json", name, - ["rf_pos_x","rf_pos_y","rf_pos_z","rf_ori_x","rf_ori_y","rf_ori_z","rf_ori_q", - "lf_pos_x","lf_pos_y","lf_pos_z","lf_ori_x","lf_ori_y","lf_ori_z","lf_ori_q"] + True, + name, + "json", + name, + [ + "rf_pos_x", + "rf_pos_y", + "rf_pos_z", + "rf_ori_x", + "rf_ori_y", + "rf_ori_z", + "rf_ori_q", + "lf_pos_x", + "lf_pos_y", + "lf_pos_z", + "lf_ori_x", + "lf_ori_y", + "lf_ori_z", + "lf_ori_q", + ], ).add_chan(server) proj_footstep_chan_ids.append(foot_scene) # Add a channel for projected footstep visual elements proj_footstep_viz_chan_id = await SceneChannel( - False, name+"_viz", "protobuf", SceneUpdate.DESCRIPTOR.full_name, scene_schema + False, + name + "_viz", + "protobuf", + SceneUpdate.DESCRIPTOR.full_name, + scene_schema, ).add_chan(server) proj_footstep_viz_chan_ids.append(proj_footstep_viz_chan_id) - #add footstep visual elements + # add footstep visual elements proj_footsteps = ShapeScene() proj_footsteps.add_shape(rf, "cubes", [1, 0, 0, 1], [x, y, 0.001]) - proj_footsteps.add_shape(lf, "cubes", [.1, .5, 1, 1], [x, y, 0.001]) + proj_footsteps.add_shape(lf, "cubes", [0.1, 0.5, 1, 1], [x, y, 0.001]) proj_feet.append(proj_footsteps) arrows_scene = ShapeScene() - arrows_scene.add_shape("lfoot_rf_cmd", "arrows", [0, 0, 1, 0.5], [0.03, 0.1, 0.08]) # blue arrow - arrows_scene.add_shape("rfoot_rf_cmd", "arrows", [0, 0, 1, 0.5], [0.03, 0.1, 0.08]) # blue arrow - arrows_scene.add_shape("lfoot_rf_normal_filt", "arrows", [0.2, 0.2, 0.2, 0.5], [0.03, 0.1, 0.08]) # grey arrow - arrows_scene.add_shape("rfoot_rf_normal_filt", "arrows", [0.2, 0.2, 0.2, 0.5], [0.03, 0.1, 0.08]) # grey arrow - - #add visual icp spheres + arrows_scene.add_shape( + "lfoot_rf_cmd", "arrows", [0, 0, 1, 0.5], [0.03, 0.1, 0.08] + ) # blue arrow + arrows_scene.add_shape( + "rfoot_rf_cmd", "arrows", [0, 0, 1, 0.5], [0.03, 0.1, 0.08] + ) # blue arrow + arrows_scene.add_shape( + "lfoot_rf_normal_filt", "arrows", [0.2, 0.2, 0.2, 0.5], [0.03, 0.1, 0.08] + ) # grey arrow + arrows_scene.add_shape( + "rfoot_rf_normal_filt", "arrows", [0.2, 0.2, 0.2, 0.5], [0.03, 0.1, 0.08] + ) # grey arrow + + # add visual icp spheres icp_spheres = ShapeScene() icp_spheres.add_shape("est_icp", "spheres", [1, 0, 1, 1], [0.1, 0.1, 0.1]) icp_spheres.add_shape("des_icp", "spheres", [0, 1, 0, 1], [0.1, 0.1, 0.1]) @@ -351,12 +382,22 @@ async def main(): now, json.dumps( { - "rf_pos_x": r_pos[0],"rf_pos_y": r_pos[1],"rf_pos_z": r_pos[2], - "rf_ori_x": r_ori[1],"rf_ori_y": r_ori[2],"rf_ori_z": r_ori[3],"rf_ori_q": r_ori[0], - "lf_pos_x": l_pos[0],"lf_pos_y": l_pos[1],"lf_pos_z": l_pos[2], - "lf_ori_x": l_ori[1],"lf_ori_y": l_ori[2],"lf_ori_z": l_ori[3],"lf_ori_q": l_ori[0] + "rf_pos_x": r_pos[0], + "rf_pos_y": r_pos[1], + "rf_pos_z": r_pos[2], + "rf_ori_x": r_ori[1], + "rf_ori_y": r_ori[2], + "rf_ori_z": r_ori[3], + "rf_ori_q": r_ori[0], + "lf_pos_x": l_pos[0], + "lf_pos_y": l_pos[1], + "lf_pos_z": l_pos[2], + "lf_ori_x": l_ori[1], + "lf_ori_y": l_ori[2], + "lf_ori_z": l_ori[3], + "lf_ori_q": l_ori[0], } - ).encode("utf8") + ).encode("utf8"), ) for scn in xyz_scenes: @@ -464,9 +505,9 @@ async def main(): # force scale force_magnitude = force_magnitude / 1200.0 arrows_scene.scale(obj, quat_force, force_magnitude, now) - tasks.append(server.send_message( - tf_chan_id, now, transform.SerializeToString() - )) + tasks.append( + server.send_message(tf_chan_id, now, transform.SerializeToString()) + ) await server.send_message( normS_chan_id, now, arrows_scene.serialized_msg(obj) ) @@ -479,22 +520,32 @@ async def main(): transform.translation.x = list(getattr(msg, obj))[0] transform.translation.y = list(getattr(msg, obj))[1] icp_spheres.update(obj, now) - tasks.append(server.send_message( - tf_chan_id, now, transform.SerializeToString() - )) - tasks.append(server.send_message( - icpS_chan_id, now, icp_spheres.serialized_msg(obj) - )) + tasks.append( + server.send_message(tf_chan_id, now, transform.SerializeToString()) + ) + tasks.append( + server.send_message( + icpS_chan_id, now, icp_spheres.serialized_msg(obj) + ) + ) # Update projected footsteps update = fp.sd.steps_to_update() for obj in msgs: if obj in update: - stepnum = int(''.join(filter(lambda i: i.isdigit(), obj))) + stepnum = int("".join(filter(lambda i: i.isdigit(), obj))) yaml = fp.sd.yaml_num - setattr(fp.sd, obj[5] + "f_steps_taken", getattr(fp.sd, obj[5] + "f_steps_taken") + 1) - proj_foot_pos[obj] = getattr(fp, obj[5] + "foot_contact_pos")[yaml][update[obj]] - proj_foot_ori[obj] = getattr(fp, obj[5] + "foot_contact_ori")[yaml][update[obj]] + setattr( + fp.sd, + obj[5] + "f_steps_taken", + getattr(fp.sd, obj[5] + "f_steps_taken") + 1, + ) + proj_foot_pos[obj] = getattr(fp, obj[5] + "foot_contact_pos")[yaml][ + update[obj] + ] + proj_foot_ori[obj] = getattr(fp, obj[5] + "foot_contact_ori")[yaml][ + update[obj] + ] transform.parent_frame_id = "world" transform.child_frame_id = obj transform.timestamp.FromNanoseconds(now) @@ -506,12 +557,18 @@ async def main(): transform.rotation.z = proj_foot_ori[obj][3] transform.rotation.w = proj_foot_ori[obj][0] proj_feet[stepnum].update(obj, now) - tasks.append(server.send_message( - tf_chan_id, now, transform.SerializeToString() - )) - tasks.append(server.send_message( - proj_footstep_viz_chan_ids[stepnum], now, proj_feet[stepnum].serialized_msg(obj) - )) + tasks.append( + server.send_message( + tf_chan_id, now, transform.SerializeToString() + ) + ) + tasks.append( + server.send_message( + proj_footstep_viz_chan_ids[stepnum], + now, + proj_feet[stepnum].serialized_msg(obj), + ) + ) await asyncio.gather(*tasks) diff --git a/UI/foxglove/client/parameter_subscriber.cpp b/UI/foxglove/client/parameter_subscriber.cpp index fb25252f..357776d1 100644 --- a/UI/foxglove/client/parameter_subscriber.cpp +++ b/UI/foxglove/client/parameter_subscriber.cpp @@ -6,7 +6,8 @@ FoxgloveParameterSubscriber::FoxgloveParameterSubscriber( std::unordered_map ¶meters_map_int, std::unordered_map ¶meters_map_double, std::unordered_map ¶meters_map_task, - std::unordered_map ¶meters_map_hm) { + std::unordered_map + ¶meters_map_hm) { next_sub_id_ = 0; client_.setBinaryMessageHandler([&](const uint8_t *data, size_t dataLength) { @@ -92,7 +93,8 @@ FoxgloveParameterSubscriber::FoxgloveParameterSubscriber( }); const auto openHandler = [&](websocketpp::connection_hdl) { - std::cout << "[FoxgloveParameterSubscriber] Connected to " << std::string(url) << std::endl; + std::cout << "[FoxgloveParameterSubscriber] Connected to " + << std::string(url) << std::endl; }; const auto closeHandler = [&](websocketpp::connection_hdl) { std::cout << "[FoxgloveParameterSubscriber] Connection closed" << std::endl; diff --git a/UI/foxglove/footstep_planner.py b/UI/foxglove/footstep_planner.py index fb43cda2..d04b1c4e 100644 --- a/UI/foxglove/footstep_planner.py +++ b/UI/foxglove/footstep_planner.py @@ -7,41 +7,48 @@ import numpy as np # Define the directory to watch -WATCHED_DIR = 'experiment_data' +WATCHED_DIR = "experiment_data" # variables loaded from yaml t_ini_footsteps_planned, t_end_footsteps_planned = [], [] rfoot_contact_pos, rfoot_contact_ori = [], [] lfoot_contact_pos, lfoot_contact_ori = [], [] + class StepData: - def __init__(self, yaml_num, rf_steps_taken, rf_steps_total, lf_steps_taken, lf_steps_total): + def __init__( + self, yaml_num, rf_steps_taken, rf_steps_total, lf_steps_taken, lf_steps_total + ): self.yaml_num = yaml_num self.rf_steps_taken = rf_steps_taken self.rf_steps_total = rf_steps_total self.lf_steps_taken = lf_steps_taken self.lf_steps_total = lf_steps_total + def step_update(self, res, r_len, l_len): self.yaml_num = res self.rf_steps_taken = self.rf_steps_total self.lf_steps_taken = self.lf_steps_total self.rf_steps_total += r_len self.lf_steps_total += l_len + def steps_to_update(self): # Returns dict of projections mapped to the yaml index feet = {} for i in range(self.rf_steps_taken, self.rf_steps_total): - rf = "proj_rf"+str(i) - feet[rf] = i-self.rf_steps_taken + rf = "proj_rf" + str(i) + feet[rf] = i - self.rf_steps_taken for i in range(self.lf_steps_taken, self.lf_steps_total): - lf = "proj_lf"+str(i) - feet[lf] = i-self.lf_steps_taken + lf = "proj_lf" + str(i) + feet[lf] = i - self.lf_steps_taken return feet -sd = StepData(0,0,0,0,0) + +sd = StepData(0, 0, 0, 0, 0) + def yaml_check(file_path, search_line): try: - with open(file_path, 'r') as file: + with open(file_path, "r") as file: lines = file.readlines() # Check if the search_line exists in the list of lines for line in lines: @@ -55,11 +62,12 @@ def yaml_check(file_path, search_line): print(f"An error occurred: {e}") return False + def remove_yaml_files(directory): if not os.path.isdir(directory): raise ValueError(f"The directory {directory} does not exist") # Use glob to find all .yaml files in the directory - yaml_files = glob.glob(os.path.join(directory, '*.yaml')) + yaml_files = glob.glob(os.path.join(directory, "*.yaml")) # Remove each .yaml file found print("*** removing old experiment data ***") for file_path in yaml_files: @@ -70,10 +78,11 @@ def remove_yaml_files(directory): print(f"Error removing file {file_path}: {e}") print("*** old experiment data removed ***") + class yamlHandler(FileSystemEventHandler): def on_created(self, event): # Check if the created file is a YAML file - if event.is_directory or not event.src_path.endswith('.yaml'): + if event.is_directory or not event.src_path.endswith(".yaml"): return print(f"New YAML file detected: {event.src_path}") self.process_file(event.src_path) @@ -81,20 +90,31 @@ def on_created(self, event): def process_file(self, file_path): with open(file_path, "r") as stream: try: - while not yaml_check(file_path, " vrp:"): # Wait until yaml data is generated + while not yaml_check( + file_path, " vrp:" + ): # Wait until yaml data is generated time.sleep(0.01) cfg = yaml.load(stream, Loader=yaml.FullLoader) - t_ini_footsteps_planned.append(np.array(cfg["temporal_parameters"]["initial_time"])) - t_end_footsteps_planned.append(np.array(cfg["temporal_parameters"]["final_time"])) + t_ini_footsteps_planned.append( + np.array(cfg["temporal_parameters"]["initial_time"]) + ) + t_end_footsteps_planned.append( + np.array(cfg["temporal_parameters"]["final_time"]) + ) rfoot_contact_pos.append(np.array(cfg["contact"]["right_foot"]["pos"])) rfoot_contact_ori.append(np.array(cfg["contact"]["right_foot"]["ori"])) lfoot_contact_pos.append(np.array(cfg["contact"]["left_foot"]["pos"])) lfoot_contact_ori.append(np.array(cfg["contact"]["left_foot"]["ori"])) - res = ''.join(filter(lambda i: i.isdigit(), str(file_path))) - sd.step_update(int(res), len(rfoot_contact_pos[int(res)]), len(lfoot_contact_pos[int(res)])) + res = "".join(filter(lambda i: i.isdigit(), str(file_path))) + sd.step_update( + int(res), + len(rfoot_contact_pos[int(res)]), + len(lfoot_contact_pos[int(res)]), + ) except yaml.YAMLError as exc: print(exc) + def main(): remove_yaml_files(WATCHED_DIR) event_handler = yamlHandler() @@ -108,5 +128,6 @@ def main(): observer.stop() observer.join() + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/UI/visualization_toolbox.py b/UI/visualization_toolbox.py index 06f234f1..18d92b8c 100644 --- a/UI/visualization_toolbox.py +++ b/UI/visualization_toolbox.py @@ -17,10 +17,13 @@ def isMesh(geometry_object): return False -def update_robot_transform(visual: pin.GeometryModel, - visual_data: pin.GeometryData, - visual_model: pin.GeometryModel, - transform: FrameTransform): + +def update_robot_transform( + visual: pin.GeometryModel, + visual_data: pin.GeometryData, + visual_model: pin.GeometryModel, + transform: FrameTransform, +): parent_name = "world" # Get mesh pose. M = visual_data.oMg[visual_model.getGeometryId(visual.name)] @@ -44,11 +47,9 @@ def update_robot_transform(visual: pin.GeometryModel, transform.rotation.z = q[2] transform.rotation.w = q[3] -def update_2d_transform(obj_name: str, - pos_2d: np.ndarray, - transform: FrameTransform): + +def update_2d_transform(obj_name: str, pos_2d: np.ndarray, transform: FrameTransform): transform.parent_frame_id = "world" transform.child_frame_id = obj_name transform.translation.x = pos_2d[0] transform.translation.y = pos_2d[1] - \ No newline at end of file diff --git a/config/draco/hw/ihwbc/MPC_LOCOMOTION.yaml b/config/draco/hw/ihwbc/MPC_LOCOMOTION.yaml index e1043894..9e60f420 100644 --- a/config/draco/hw/ihwbc/MPC_LOCOMOTION.yaml +++ b/config/draco/hw/ihwbc/MPC_LOCOMOTION.yaml @@ -82,4 +82,3 @@ gait: ## 0: standing ## 1: walking gait_number: 1 - diff --git a/config/draco/hw/ihwbc/pnc.yaml b/config/draco/hw/ihwbc/pnc.yaml index e02ab867..b136b4b2 100644 --- a/config/draco/hw/ihwbc/pnc.yaml +++ b/config/draco/hw/ihwbc/pnc.yaml @@ -23,7 +23,7 @@ contact_detection: state_estimator: b_cheater_mode: false ## true: use simulation groundtruth states - state_estimator_type: default ## 1. default (IMU + Leg kinematics) 2. kf + state_estimator_type: default ## 1. default (IMU + Leg kinematics) 2. kf b_use_marg_filter: false # only used when kf is set to true foot_reference_frame: 1 #0: left foot, 1: right foot @@ -53,7 +53,7 @@ state_estimator: num_data_com_vel: [200, 200, 200] ## IF 1: exponential smoother filter parameter - #com_vel_time_constant: 0.1 + #com_vel_time_constant: 0.1 com_vel_time_constant: 0.01 com_vel_err_limit: [1., 1., 1.] @@ -62,7 +62,7 @@ state_estimator: controller: - b_smoothing_command: true + b_smoothing_command: true #smoothing_command_duration: 0.2 smoothing_command_duration: 5. ## this will filter the control command at the beginning b_use_modified_swing_foot_jac: true diff --git a/config/draco/hw/wbic/MPC_LOCOMOTION.yaml b/config/draco/hw/wbic/MPC_LOCOMOTION.yaml index e1043894..9e60f420 100644 --- a/config/draco/hw/wbic/MPC_LOCOMOTION.yaml +++ b/config/draco/hw/wbic/MPC_LOCOMOTION.yaml @@ -82,4 +82,3 @@ gait: ## 0: standing ## 1: walking gait_number: 1 - diff --git a/config/draco/hw/wbic/nodelet.yaml b/config/draco/hw/wbic/nodelet.yaml index ae03e2ed..95415f48 100644 --- a/config/draco/hw/wbic/nodelet.yaml +++ b/config/draco/hw/wbic/nodelet.yaml @@ -182,4 +182,3 @@ service_call: weak_kp: 7.0 weak_kd: 0.07 weak_current_limit: 0.03 - diff --git a/config/draco/hw/wbic/pnc.yaml b/config/draco/hw/wbic/pnc.yaml index 6a4bde8d..fbd8fcd0 100644 --- a/config/draco/hw/wbic/pnc.yaml +++ b/config/draco/hw/wbic/pnc.yaml @@ -8,7 +8,7 @@ stance_foot: 1 # 0: left_foot 1: right_foot state_estimator: b_cheater_mode: false ## true: use simulation groundtruth states - state_estimator_type: default ## 1. default (IMU + Leg kinematics) 2. kf + state_estimator_type: default ## 1. default (IMU + Leg kinematics) 2. kf sigma_base_vel: [0.0, 0.0, 0.0] sigma_base_acc: [0.5, 0.5, 0.5] @@ -29,7 +29,7 @@ state_estimator: num_data_com_vel: [10, 10, 10] ## IF 1: exponential smoother filter parameter - #com_vel_time_constant: 0.1 + #com_vel_time_constant: 0.1 com_vel_time_constant: 0.01 com_vel_err_limit: [1., 1., 1.] @@ -38,7 +38,7 @@ state_estimator: controller: - b_smoothing_command: true + b_smoothing_command: true #smoothing_command_duration: 0.2 smoothing_command_duration: 3. ## this will filter the control command at the beginning b_use_modified_swing_foot_jac: true @@ -57,7 +57,7 @@ wbc: icp_kp : [2.0, 2.0] icp_kd : [0., 0.] # x, y will be ignored - icp_ki : [0.0, 0.0] + icp_ki : [0.0, 0.0] kp_ik : [1.0, 1.0] #kp_ik : [0.7, 0.7] diff --git a/config/draco/sim/mujoco/ihwbc/MPC_LOCOMOTION.yaml b/config/draco/sim/mujoco/ihwbc/MPC_LOCOMOTION.yaml index 4378a614..3bc2c787 100644 --- a/config/draco/sim/mujoco/ihwbc/MPC_LOCOMOTION.yaml +++ b/config/draco/sim/mujoco/ihwbc/MPC_LOCOMOTION.yaml @@ -82,4 +82,3 @@ gait: ## 0: standing ## 1: walking gait_number: 1 - diff --git a/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml b/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml index ca90f681..d2285de0 100644 --- a/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml +++ b/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml @@ -1,6 +1,6 @@ actuator_gains: - l_hip_ie: - kp: 400 + l_hip_ie: + kp: 400 kd: 20 l_hip_aa: kp: 500 @@ -18,7 +18,7 @@ actuator_gains: kp: 40 kd: 2 l_shoulder_fe: - kp: 100 + kp: 100 kd: 5 l_shoulder_aa: kp: 100 @@ -39,7 +39,7 @@ actuator_gains: kp: 10 kd: 5 neck_pitch: - kp: 100 + kp: 100 kd: 5 r_hip_ie: kp: 400 @@ -89,7 +89,7 @@ initial_config: base_quat_x: 0. base_quat_y: 0. base_quat_z: 0. - l_hip_ie: 0. + l_hip_ie: 0. l_hip_aa: 0. l_hip_fe: 0 l_knee_fe_jp: 0 @@ -118,4 +118,3 @@ initial_config: r_wrist_ps: 0. r_wrist_pitch: 0. right_ezgripper_knuckle_palm_L1_1: 0. - diff --git a/config/draco/sim/mujoco/ihwbc/pnc.yaml b/config/draco/sim/mujoco/ihwbc/pnc.yaml index af60e4b4..413bdb9d 100644 --- a/config/draco/sim/mujoco/ihwbc/pnc.yaml +++ b/config/draco/sim/mujoco/ihwbc/pnc.yaml @@ -24,7 +24,7 @@ contact_detection: state_estimator: b_cheater_mode: true ## true: use simulation groundtruth states - state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf + state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf b_use_marg_filter: false # only used when kf is set to true foot_reference_frame: 1 #0: left foot, 1: right foot @@ -50,7 +50,7 @@ state_estimator: num_data_com_vel: [20, 20, 20] ## IF 1: exponential smoother filter parameter - #com_vel_time_constant: 0.1 + #com_vel_time_constant: 0.1 com_vel_time_constant: 0.01 com_vel_err_limit: [1., 1., 1.] @@ -59,7 +59,7 @@ state_estimator: controller: - b_smoothing_command: true + b_smoothing_command: true smoothing_command_duration: 1.0 ## this will filter the control command at the beginning b_use_modified_swing_foot_jac: true b_use_modified_hand_jac: false @@ -155,7 +155,7 @@ wbc: mu: 0.4 foot_half_length: 0.08 foot_half_width: 0.04 ## 0.04 - kp: [0, 0, 0, 0, 0, 0] ## ori, pos order + kp: [0, 0, 0, 0, 0, 0] ## ori, pos order kd: [0, 0, 0, 0, 0, 0] qp: diff --git a/config/draco/sim/mujoco/wbic/MPC_LOCOMOTION.yaml b/config/draco/sim/mujoco/wbic/MPC_LOCOMOTION.yaml index 4378a614..3bc2c787 100644 --- a/config/draco/sim/mujoco/wbic/MPC_LOCOMOTION.yaml +++ b/config/draco/sim/mujoco/wbic/MPC_LOCOMOTION.yaml @@ -82,4 +82,3 @@ gait: ## 0: standing ## 1: walking gait_number: 1 - diff --git a/config/draco/sim/mujoco/wbic/mujoco_params.yaml b/config/draco/sim/mujoco/wbic/mujoco_params.yaml index 2afc1c0b..a81517b4 100644 --- a/config/draco/sim/mujoco/wbic/mujoco_params.yaml +++ b/config/draco/sim/mujoco/wbic/mujoco_params.yaml @@ -1,6 +1,6 @@ actuator_gains: - l_hip_ie: - kp: 400 + l_hip_ie: + kp: 400 kd: 20 l_hip_aa: kp: 500 @@ -18,7 +18,7 @@ actuator_gains: kp: 40 kd: 2 l_shoulder_fe: - kp: 100 + kp: 100 kd: 5 l_shoulder_aa: kp: 100 @@ -36,7 +36,7 @@ actuator_gains: kp: 100 kd: 5 neck_pitch: - kp: 100 + kp: 100 kd: 5 r_hip_ie: kp: 400 @@ -83,7 +83,7 @@ initial_config: base_quat_x: 0. base_quat_y: 0. base_quat_z: 0. - l_hip_ie: 0. + l_hip_ie: 0. l_hip_aa: 0. l_hip_fe: 0 l_knee_fe_jp: 0 @@ -110,4 +110,3 @@ initial_config: r_elbow_fe: 0. r_wrist_ps: 0. r_wrist_pitch: 0. - diff --git a/config/draco/sim/mujoco/wbic/pnc.yaml b/config/draco/sim/mujoco/wbic/pnc.yaml index c6a73039..a3e1a1a5 100644 --- a/config/draco/sim/mujoco/wbic/pnc.yaml +++ b/config/draco/sim/mujoco/wbic/pnc.yaml @@ -8,7 +8,7 @@ stance_foot: 1 # 0: left_foot 1: right_foot state_estimator: b_cheater_mode: true ## true: use simulation groundtruth states - state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf + state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf b_use_marg_filter: false # only used when kf is set to true foot_reference_frame: 1 #0: left foot, 1: right foot @@ -35,7 +35,7 @@ state_estimator: num_data_com_vel: [10, 10, 10] ## IF 1: exponential smoother filter parameter - #com_vel_time_constant: 0.1 + #com_vel_time_constant: 0.1 com_vel_time_constant: 0.01 com_vel_err_limit: [1., 1., 1.] @@ -44,7 +44,7 @@ state_estimator: controller: - b_smoothing_command: true + b_smoothing_command: true smoothing_command_duration: 1.0 ## this will filter the control command at the beginning b_use_modified_swing_foot_jac: true b_use_modified_hand_jac: true @@ -64,7 +64,7 @@ wbc: icp_kp : [4.0, 4.0] icp_kd : [0., 0.] # x, y will be ignored - icp_ki : [0.0, 0.0] + icp_ki : [0.0, 0.0] kp_ik : [1.0, 1.0] @@ -275,5 +275,3 @@ lmpc_walking: nominal_strafe_distance: 0.02 n_steps: 1 first_swing_leg: 0 ## 0: left leg ## 1: right leg - - diff --git a/config/draco/sim/pybullet/ihwbc/MPC_LOCOMOTION.yaml b/config/draco/sim/pybullet/ihwbc/MPC_LOCOMOTION.yaml index e1043894..9e60f420 100644 --- a/config/draco/sim/pybullet/ihwbc/MPC_LOCOMOTION.yaml +++ b/config/draco/sim/pybullet/ihwbc/MPC_LOCOMOTION.yaml @@ -82,4 +82,3 @@ gait: ## 0: standing ## 1: walking gait_number: 1 - diff --git a/config/draco/sim/pybullet/ihwbc/pnc.yaml b/config/draco/sim/pybullet/ihwbc/pnc.yaml index a937249e..26d1a348 100644 --- a/config/draco/sim/pybullet/ihwbc/pnc.yaml +++ b/config/draco/sim/pybullet/ihwbc/pnc.yaml @@ -24,7 +24,7 @@ contact_detection: state_estimator: b_cheater_mode: true ## true: use simulation groundtruth states - state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf + state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf b_use_marg_filter: false # only used when kf is set to true foot_reference_frame: 1 #0: left foot, 1: right foot @@ -50,7 +50,7 @@ state_estimator: num_data_com_vel: [20, 20, 20] ## IF 1: exponential smoother filter parameter - #com_vel_time_constant: 0.1 + #com_vel_time_constant: 0.1 com_vel_time_constant: 0.05 com_vel_err_limit: [1., 1., 1.] @@ -59,7 +59,7 @@ state_estimator: controller: - b_smoothing_command: false + b_smoothing_command: false smoothing_command_duration: 0.01 ## this will filter the control command at the beginning b_use_modified_swing_foot_jac: true b_use_modified_hand_jac: true @@ -154,7 +154,7 @@ wbc: mu: 0.4 foot_half_length: 0.08 foot_half_width: 0.04 ## 0.04 - kp: [0, 0, 0, 0, 0, 0] ## ori, pos order + kp: [0, 0, 0, 0, 0, 0] ## ori, pos order kd: [0, 0, 0, 0, 0, 0] qp: diff --git a/config/draco/sim/pybullet/ihwbc/pybullet_params.py b/config/draco/sim/pybullet/ihwbc/pybullet_params.py index acaa431d..974ab15f 100644 --- a/config/draco/sim/pybullet/ihwbc/pybullet_params.py +++ b/config/draco/sim/pybullet/ihwbc/pybullet_params.py @@ -182,12 +182,65 @@ class DracoManipulationJointIdx(object): class JointGains(object): # kp = 5. * np.ones(27) # kd = 0. * np.ones(27) - kp = np.array([ - 10, 10, 10, 10, 10, 10, 10, 5, 5, 5, 5, 5, 5, 5, 10, 10, 10, 10, 10, - 10, 10, 5, 5, 5, 5, 5, 5 - ]) - kd = np.array([ - 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.001, 0.001, 0.001, 0.001, - 0.001, 0.001, 0.01, 0.01, 0.001, 0.01, 0.01, 0.01, 0.01, 0.01, 0.001, - 0.001, 0.001, 0.001, 0.001, 0.001 - ]) + kp = np.array( + [ + 10, + 10, + 10, + 10, + 10, + 10, + 10, + 5, + 5, + 5, + 5, + 5, + 5, + 5, + 10, + 10, + 10, + 10, + 10, + 10, + 10, + 5, + 5, + 5, + 5, + 5, + 5, + ] + ) + kd = np.array( + [ + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.01, + 0.01, + 0.001, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + ] + ) diff --git a/config/draco/sim/pybullet/wbic/MPC_LOCOMOTION.yaml b/config/draco/sim/pybullet/wbic/MPC_LOCOMOTION.yaml index e1043894..9e60f420 100644 --- a/config/draco/sim/pybullet/wbic/MPC_LOCOMOTION.yaml +++ b/config/draco/sim/pybullet/wbic/MPC_LOCOMOTION.yaml @@ -82,4 +82,3 @@ gait: ## 0: standing ## 1: walking gait_number: 1 - diff --git a/config/draco/sim/pybullet/wbic/pnc.yaml b/config/draco/sim/pybullet/wbic/pnc.yaml index c04b06dc..35ae5895 100644 --- a/config/draco/sim/pybullet/wbic/pnc.yaml +++ b/config/draco/sim/pybullet/wbic/pnc.yaml @@ -8,7 +8,7 @@ stance_foot: 1 # 0: left_foot 1: right_foot state_estimator: b_cheater_mode: true ## true: use simulation groundtruth states - state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf + state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf b_use_marg_filter: false # only used when kf is set to true foot_reference_frame: 1 #0: left foot, 1: right foot @@ -34,7 +34,7 @@ state_estimator: num_data_com_vel: [10, 10, 10] ## IF 1: exponential smoother filter parameter - #com_vel_time_constant: 0.1 + #com_vel_time_constant: 0.1 com_vel_time_constant: 0.01 com_vel_err_limit: [1., 1., 1.] @@ -43,7 +43,7 @@ state_estimator: controller: - b_smoothing_command: true + b_smoothing_command: true smoothing_command_duration: 0.01 ## this will filter the control command at the beginning b_use_modified_swing_foot_jac: true b_use_modified_hand_jac: false @@ -61,7 +61,7 @@ wbc: icp_kp : [2, 2] icp_kd : [0., 0.] # x, y will be ignored - icp_ki : [0., 0.] + icp_ki : [0., 0.] kp_ik : [1., 1.] #kp_ik : [0.5, 0.5] @@ -143,7 +143,7 @@ wbc: mu: 0.5 foot_half_length: 0.08 foot_half_width: 0.04 ## 0.04 - kp: [0, 0, 0, 0, 0, 0] ## ori, pos order + kp: [0, 0, 0, 0, 0, 0] ## ori, pos order kd: [0, 0, 0, 0, 0, 0] #kp: [100, 100, 100, 100, 100, 100] ## ori, pos order #kd: [20, 20, 20, 20, 20, 20] diff --git a/config/draco/sim/pybullet/wbic/pybullet_params.py b/config/draco/sim/pybullet/wbic/pybullet_params.py index caa374c5..374e3b9b 100644 --- a/config/draco/sim/pybullet/wbic/pybullet_params.py +++ b/config/draco/sim/pybullet/wbic/pybullet_params.py @@ -182,12 +182,65 @@ class PybulletDracoJointIdx(object): class JointGains(object): # kp = 5. * np.ones(27) # kd = 0. * np.ones(27) - kp = np.array([ - 10, 10, 10, 10, 10, 10, 10, 5, 5, 5, 5, 5, 5, 5, 10, 10, 10, 10, 10, - 10, 10, 5, 5, 5, 5, 5, 5 - ]) - kd = np.array([ - 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.001, 0.001, 0.001, 0.001, - 0.001, 0.001, 0.01, 0.01, 0.001, 0.01, 0.01, 0.01, 0.01, 0.01, 0.001, - 0.001, 0.001, 0.001, 0.001, 0.001 - ]) + kp = np.array( + [ + 10, + 10, + 10, + 10, + 10, + 10, + 10, + 5, + 5, + 5, + 5, + 5, + 5, + 5, + 10, + 10, + 10, + 10, + 10, + 10, + 10, + 5, + 5, + 5, + 5, + 5, + 5, + ] + ) + kd = np.array( + [ + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.01, + 0.01, + 0.001, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + ] + ) diff --git a/controller/control_architecture.hpp b/controller/control_architecture.hpp index 3ca33ce4..3196d55b 100644 --- a/controller/control_architecture.hpp +++ b/controller/control_architecture.hpp @@ -8,7 +8,7 @@ class ControlArchitecture { public: ControlArchitecture(PinocchioRobotSystem *robot) : robot_(robot), b_loco_state_first_visit_(true), - b_manip_state_first_visit_(true){}; + b_manip_state_first_visit_(true) {}; virtual ~ControlArchitecture() = default; virtual void GetCommand(void *command) = 0; diff --git a/controller/draco_controller/draco_crbi/draco3_crbi_helper.h b/controller/draco_controller/draco_crbi/draco3_crbi_helper.h index 985197c6..d572fdf9 100644 --- a/controller/draco_controller/draco_crbi/draco3_crbi_helper.h +++ b/controller/draco_controller/draco_crbi/draco3_crbi_helper.h @@ -1,5 +1,5 @@ /* This file was automatically generated by CasADi 3.6.4. - * It consists of: + * 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 @@ -17,7 +17,8 @@ extern "C" { #define casadi_int long long int #endif -int draco3_crbi_helper(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +int draco3_crbi_helper(const casadi_real **arg, casadi_real **res, + casadi_int *iw, casadi_real *w, int mem); int draco3_crbi_helper_alloc_mem(void); int draco3_crbi_helper_init_mem(int mem); void draco3_crbi_helper_free_mem(int mem); @@ -28,16 +29,18 @@ void draco3_crbi_helper_decref(void); casadi_int draco3_crbi_helper_n_in(void); casadi_int draco3_crbi_helper_n_out(void); casadi_real draco3_crbi_helper_default_in(casadi_int i); -const char* draco3_crbi_helper_name_in(casadi_int i); -const char* draco3_crbi_helper_name_out(casadi_int i); -const casadi_int* draco3_crbi_helper_sparsity_in(casadi_int i); -const casadi_int* draco3_crbi_helper_sparsity_out(casadi_int i); -int draco3_crbi_helper_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +const char *draco3_crbi_helper_name_in(casadi_int i); +const char *draco3_crbi_helper_name_out(casadi_int i); +const casadi_int *draco3_crbi_helper_sparsity_in(casadi_int i); +const casadi_int *draco3_crbi_helper_sparsity_out(casadi_int i); +int draco3_crbi_helper_work(casadi_int *sz_arg, casadi_int *sz_res, + casadi_int *sz_iw, casadi_int *sz_w); #define draco3_crbi_helper_SZ_ARG 10 #define draco3_crbi_helper_SZ_RES 2 #define draco3_crbi_helper_SZ_IW 0 #define draco3_crbi_helper_SZ_W 5626 -int jac_draco3_crbi_helper(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +int jac_draco3_crbi_helper(const casadi_real **arg, casadi_real **res, + casadi_int *iw, casadi_real *w, int mem); int jac_draco3_crbi_helper_alloc_mem(void); int jac_draco3_crbi_helper_init_mem(int mem); void jac_draco3_crbi_helper_free_mem(int mem); @@ -48,11 +51,12 @@ void jac_draco3_crbi_helper_decref(void); casadi_int jac_draco3_crbi_helper_n_in(void); casadi_int jac_draco3_crbi_helper_n_out(void); casadi_real jac_draco3_crbi_helper_default_in(casadi_int i); -const char* jac_draco3_crbi_helper_name_in(casadi_int i); -const char* jac_draco3_crbi_helper_name_out(casadi_int i); -const casadi_int* jac_draco3_crbi_helper_sparsity_in(casadi_int i); -const casadi_int* jac_draco3_crbi_helper_sparsity_out(casadi_int i); -int jac_draco3_crbi_helper_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +const char *jac_draco3_crbi_helper_name_in(casadi_int i); +const char *jac_draco3_crbi_helper_name_out(casadi_int i); +const casadi_int *jac_draco3_crbi_helper_sparsity_in(casadi_int i); +const casadi_int *jac_draco3_crbi_helper_sparsity_out(casadi_int i); +int jac_draco3_crbi_helper_work(casadi_int *sz_arg, casadi_int *sz_res, + casadi_int *sz_iw, casadi_int *sz_w); #define jac_draco3_crbi_helper_SZ_ARG 11 #define jac_draco3_crbi_helper_SZ_RES 12 #define jac_draco3_crbi_helper_SZ_IW 0 diff --git a/controller/draco_controller/draco_data_manager.hpp b/controller/draco_controller/draco_data_manager.hpp index bb28b27c..804f4edb 100644 --- a/controller/draco_controller/draco_data_manager.hpp +++ b/controller/draco_controller/draco_data_manager.hpp @@ -8,7 +8,7 @@ struct DracoData { public: - DracoData(){}; + DracoData() {}; ~DracoData() = default; double time_ = 0; diff --git a/controller/draco_controller/draco_interface.cpp b/controller/draco_controller/draco_interface.cpp index c6d1866c..369d4a34 100644 --- a/controller/draco_controller/draco_interface.cpp +++ b/controller/draco_controller/draco_interface.cpp @@ -30,9 +30,10 @@ DracoInterface::DracoInterface() : Interface() { std::vector unactuated_joint_list = {"l_knee_fe_jp", "r_knee_fe_jp"}; robot_ = new PinocchioRobotSystem( - THIS_COM "robot_model/draco/draco_latest_collisions.urdf", - /*This is for draco with grippers*/ -// THIS_COM "robot_model/draco/draco_latest_collisions_with_gripper.urdf", + THIS_COM "robot_model/draco/draco_latest_collisions.urdf", + /*This is for draco with grippers*/ + // THIS_COM + // "robot_model/draco/draco_latest_collisions_with_gripper.urdf", THIS_COM "robot_model/draco", false, false, &unactuated_joint_list); // robot_ = new PinocchioRobotSystem( // THIS_COM "robot_model/draco/draco_modified.urdf", diff --git a/controller/draco_controller/draco_interface.hpp b/controller/draco_controller/draco_interface.hpp index 4172a89d..a318cd5d 100644 --- a/controller/draco_controller/draco_interface.hpp +++ b/controller/draco_controller/draco_interface.hpp @@ -21,7 +21,7 @@ class DracoSensorData { b_rf_contact_(false), lf_contact_normal_(0.), rf_contact_normal_(0.), base_joint_pos_(Eigen::Vector3d::Zero()), base_joint_quat_(0, 0, 0, 1), base_joint_lin_vel_(Eigen::Vector3d::Zero()), - base_joint_ang_vel_(Eigen::Vector3d::Zero()){}; + base_joint_ang_vel_(Eigen::Vector3d::Zero()) {}; virtual ~DracoSensorData() = default; Eigen::Vector4d imu_frame_quat_; // x, y, z, w order @@ -47,7 +47,7 @@ class DracoCommand { DracoCommand() : joint_pos_cmd_(Eigen::VectorXd::Zero(draco::n_adof)), joint_vel_cmd_(Eigen::VectorXd::Zero(draco::n_adof)), - joint_trq_cmd_(Eigen::VectorXd::Zero(draco::n_adof)){}; + joint_trq_cmd_(Eigen::VectorXd::Zero(draco::n_adof)) {}; virtual ~DracoCommand() = default; Eigen::VectorXd joint_pos_cmd_; diff --git a/controller/draco_controller/draco_kf_state_estimator.cpp b/controller/draco_controller/draco_kf_state_estimator.cpp index 84b5f378..4218beda 100644 --- a/controller/draco_controller/draco_kf_state_estimator.cpp +++ b/controller/draco_controller/draco_kf_state_estimator.cpp @@ -622,7 +622,7 @@ void DracoKFStateEstimator::ComputeDCM() { sp_->dcm_ = com_pos + com_vel / dcm_omega; double cutoff_period = - 0.01; // 10ms cut-off period for first-order low pass filter + 0.01; // 10ms cut-off period for first-order low pass filter double alpha = sp_->servo_dt_ / cutoff_period; sp_->dcm_vel_ = alpha * ((sp_->dcm_ - sp_->prev_dcm_) / sp_->servo_dt_) + (1.0 - alpha) * sp_->dcm_vel_; diff --git a/controller/draco_controller/draco_kf_state_estimator.hpp b/controller/draco_controller/draco_kf_state_estimator.hpp index 1abbb180..e30d07eb 100644 --- a/controller/draco_controller/draco_kf_state_estimator.hpp +++ b/controller/draco_controller/draco_kf_state_estimator.hpp @@ -28,7 +28,7 @@ #include "controller/state_estimator/PoseMeasurementModel.hpp" #include "controller/whole_body_controller/managers/contact_detection_manager.hpp" #include "third_party/kalman_filters/ExtendedKalmanFilter.hpp" -//#include +// #include //"controller/whole_body_controller/managers/contact_detection_manager.hpp" #if B_USE_MATLOGGER diff --git a/controller/draco_controller/draco_task/draco_com_xy_task.cpp b/controller/draco_controller/draco_task/draco_com_xy_task.cpp index 36f03641..faa113e8 100644 --- a/controller/draco_controller/draco_task/draco_com_xy_task.cpp +++ b/controller/draco_controller/draco_task/draco_com_xy_task.cpp @@ -110,8 +110,8 @@ void DracoCoMXYTask::UpdateOpCommand(const Eigen::Matrix3d &world_R_local) { // TODO: clean up this // uncomment to filter ICP error before numerical integral -// icp_lpf_->Input(icp_err); -// icp_err = icp_lpf_->Output(); + // icp_lpf_->Input(icp_err); + // icp_err = icp_lpf_->Output(); // apply leaky integration icp_avg_err = icp_err * sp_->servo_dt_ + leaky_rate_ * icp_integral_; diff --git a/controller/draco_controller/draco_task/draco_wbo_task_helper.cpp b/controller/draco_controller/draco_task/draco_wbo_task_helper.cpp index 4ba3264f..6c5d2c03 100644 --- a/controller/draco_controller/draco_task/draco_wbo_task_helper.cpp +++ b/controller/draco_controller/draco_task/draco_wbo_task_helper.cpp @@ -1,5 +1,5 @@ /* This file was automatically generated by CasADi. - * It consists of: + * 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 @@ -7,11 +7,11 @@ */ /* 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) +#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) draco_wbo_task_helper_ ## ID +#define CASADI_PREFIX(ID) draco_wbo_task_helper_##ID #endif #include @@ -44,1136 +44,1854 @@ /* 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 +#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 -void casadi_clear(casadi_real* x, casadi_int n) { +void casadi_clear(casadi_real *x, casadi_int n) { casadi_int i; if (x) { - for (i=0; i(o0[3]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { +static int casadi_f0(const casadi_real **arg, casadi_real **res, casadi_int *iw, + casadi_real *w, int mem) { casadi_int i, j, k; casadi_real *rr, *ss, *tt; - casadi_real *w0=w+3, *w1=w+6, *w2=w+573, w3, w4, w5, w6, w7, w8, w9, w10, w11, w12, w13, w14, w15, w16, w17, w18, w19, w20, w21, w22, w23, w24, w25, w26, w27, w28, w29, w30, w31, w32, w33, w34, w35, w36, w37, w38, w39, w40, w41, w42, w43, w44, w45, w46, w47, w48, w49, w50, w51, w52, w53, w54, w55, w56, w57, w58, w59, w60, w61, w62, w63, w64, w65, w66, w67, w68, w69, w70, w71, w72, w73, w74, w75, w76, w77, w78, w79, w80, w81, w82, w83, w84, w85, w86, w87, w88, w89, w90, w91, w92, w93, w94, w95, w96, w97, w98, w99, w100, w101, w102, w103, w104, w105, w106, w107, w108, w109, w110, w111, w112, w113, w114, w115, w116, w117, w118, w119, w120, w121, w122, w123, w124, w125, w126, w127, w128, w129, w130, w131, w132, w133, w134, w135, w136, w137, w138, w139, w140, w141, w142, w143, w144, w145, w146, w147, w148, w149, w150, w151, w152, w153, w154, w155, w156, w157, w158, w159, w160, w161, w162, w163, w164, w165, w166, w167, w168, w169, w170, w171, w172, w173, w174, w175, w176, w177, w178, w179, w180, w181, w182, w183, w184, w185, w186, w187, w188, w189, w190, w191, *w192=w+780; + casadi_real *w0 = w + 3, *w1 = w + 6, *w2 = w + 573, w3, w4, w5, w6, w7, w8, + w9, w10, w11, w12, w13, w14, w15, w16, w17, w18, w19, w20, w21, + w22, w23, w24, w25, w26, w27, w28, w29, w30, w31, w32, w33, w34, + w35, w36, w37, w38, w39, w40, w41, w42, w43, w44, w45, w46, w47, + w48, w49, w50, w51, w52, w53, w54, w55, w56, w57, w58, w59, w60, + w61, w62, w63, w64, w65, w66, w67, w68, w69, w70, w71, w72, w73, + w74, w75, w76, w77, w78, w79, w80, w81, w82, w83, w84, w85, w86, + w87, w88, w89, w90, w91, w92, w93, w94, w95, w96, w97, w98, w99, + w100, w101, w102, w103, w104, w105, w106, w107, w108, w109, w110, + w111, w112, w113, w114, w115, w116, w117, w118, w119, w120, w121, + w122, w123, w124, w125, w126, w127, w128, w129, w130, w131, w132, + w133, w134, w135, w136, w137, w138, w139, w140, w141, w142, w143, + w144, w145, w146, w147, w148, w149, w150, w151, w152, w153, w154, + w155, w156, w157, w158, w159, w160, w161, w162, w163, w164, w165, + w166, w167, w168, w169, w170, w171, w172, w173, w174, w175, w176, + w177, w178, w179, w180, w181, w182, w183, w184, w185, w186, w187, + w188, w189, w190, w191, *w192 = w + 780; /* #0: @0 = zeros(3x1) */ casadi_clear(w0, 3); - /* #1: @1 = - [[-0.0627168, 0.00911909, 0.113161, 0.0429596, 0.0215499, -0.0190236, 0.00690652, -0.00189939, 0.002902, 0.0607266, -0.00212102, 0.12994, 0.0448556, 0.0249316, -0.00230252, -0.00362391, 0.0010149, 0.00561049, 0.00738982, 0.0688731, 0.00180948, 0.00582719, 0.00732466, -0.000265723, 0.00317307, -0.000477502, 0.000614032, -0.000308676, 0.00728648, 0.00378877, -0.000788835, -0.00171585, -0.000390278, 0.00067807, 8.72168e-05, -0.000138241, 0.000108059, 0.0157792, -0.000739303, 0.00578389, 0.00424382, 0.00213685, 0.00126477, 0.00206954, 0.00615703, 0.00169051, -0.0097154, -0.000728647, -0.00357184, -0.00681516, 0.00484937, 0.00155446, -2.22236e-05, -8.87748e-05, -0.0132386, -0.0103304, -0.00186689, -0.0010376, -0.000515239, -0.00050907, -0.00123645, 0.00840625, 0.00571376, 0.00199724, 0.00370229, 0.0084076, 0.00237273, 0.000642649, 0.00210247, -0.00912764, -0.00753473, -0.000724963, -0.00100813, -0.000475941, -2.6334e-05, 0.00132596, 0.000911836, 0.00361837, 0.00198928, 0.00132822, 0.00239418, 0.00122732, 0.000254297, 0.000857163, -0.00594194, 3.62108e-05, -0.00021935, -0.000182416, 0.000107603, 0.00269593, 0.00375798, 0.00114746, 0.000627986, -3.32492e-05, 0.000411532, 0.000765713, 0.000151893, 0.000185052, -0.00903996, 0.0135454, -0.000722359, -0.00490391, 0.00203116, 0.00710772, 0.0076969, 0.00173302, 0.00100631, 0.00420003, 0.000574205, -0.000373772, 0.00102787, -0.00561679, -0.0023325, -0.00431477, 0.00044308, 0.00322925, -0.00353879, -0.00194758, -0.000349419, -0.00114885, 0.00218443, 0.000315052, -0.000246945, -0.000234972, 0.00343182, -0.000576893, 0.00128453, -0.000627002, -0.000244513, -7.21641e-05, 0.000186332, 0.000150758, -0.000141313, 7.20824e-05, -0.00178129, 0.000312008, 0.00118627, 0.00151454, 0.000523475, 0.000436795, 0.00116518, 0.000607068, 3.05045e-05, 0.000184028, 0.0117549, 0.0796893, -0.00214423, -0.00525946, -0.00403565, 0.000691185, 0.00321344, -8.39683e-05, 0.000361989, -0.00329383, -0.010862, -0.000101139, -0.00339473, -0.00643871, 0.00571431, 0.00230936, -0.00149976, -0.00183948, -0.0151343, -0.0121317, -0.00300954, 0.00254861, 0.000666956, -0.00133131, -0.0101003, -0.00841691, -0.000414823, 0.000461035, 0.00029282, -0.000105389, -0.00673323, -0.000421961, 0.00101527, 0.000355623, -9.10409e-05, -0.00796872, -0.00509959, 0.000959568, -0.00373789, -0.00553031, -0.00328613, 0.00408343, 0.000243895, -0.00241938, -0.00116566], - [0.0186502, -0.0152338, 0.113699, 0.0327646, 0.020499, 0.108095, 0.00313986, 0.00353094, 0.0219446, 0.0203845, -0.0201273, -0.107101, -0.027541, -0.0184919, -0.11181, 0.00255492, 0.00226364, -0.0213281, 0.00436631, 0.0782532, -0.0103053, -0.00250494, -0.00615842, 0.000778518, -0.00575801, -0.000519435, -0.00156735, 0.00318815, -0.000309406, 0.00563101, 0.000204443, 5.68998e-05, -0.000929549, -0.00403978, -0.00091885, 0.000161048, -0.00316028, -0.0334966, 0.00850775, -0.0130498, -0.000829289, 0.00390585, 0.00226736, -0.00122417, 0.00455176, 0.00103396, 0.0175465, 0.000130719, 0.00269078, 0.00230771, -0.00480044, -0.000810291, -0.000815402, 0.00352587, -0.0104141, -0.00833722, -0.005023, 0.00403064, 0.00142261, -0.00163339, -0.00459653, 0.0153418, -0.00608615, -0.00163041, 0.00151932, -0.00262283, 0.0138738, 0.00181605, -0.00365686, -0.00782621, -0.00683235, -0.000510232, 0.0017275, 0.000173605, 0.000342975, -0.00261291, -0.00100338, -0.00121781, -0.00026139, -4.05719e-05, -0.00145568, 0.0055267, 0.000959155, -0.00107796, -0.00583856, 0.00117568, 0.00183215, 0.000408175, 0.00088, -0.000102079, 0.00278011, -0.00347983, -0.000377072, -0.000156108, -0.00110755, 0.00160366, 7.10275e-05, -0.000213424, 0.0112871, -0.0382876, 1.51803e-05, 0.0107112, -0.00273886, 0.00238821, 0.000870183, 0.000674078, 0.000674308, 9.9041e-05, 0.0010109, 0.000694078, -0.000421943, 0.00352808, -0.00459972, 0.0054003, 0.0056865, 0.00739394, 0.014655, 0.00521551, 0.00175918, 0.00134017, -4.93111e-05, 0.000393203, -0.000179556, 0.00324963, -0.00830727, 0.00117472, 0.00104853, 0.00224924, 0.00096319, 0.000193481, 0.000932703, -0.00048353, -1.45417e-05, -0.000183356, 0.00493695, -0.00059341, -0.00117813, 0.00318192, 0.000904924, 9.0911e-06, 0.000364803, -0.00028243, -0.000141499, -4.08834e-05, -0.00241024, -0.062841, -0.0194669, -0.00606772, -0.00765537, 0.000140633, 0.0063877, -0.000249763, -0.00191831, 0.00376216, -0.034627, 0.00946453, -0.0129204, -0.000289844, -0.00292233, -0.00266214, -0.0014625, -0.0102087, 0.0042977, 0.00396114, 0.0021332, 0.00494168, 0.00123231, 0.000931988, 0.00571952, 0.00509258, -0.00024296, 0.00221941, 0.000214861, -0.000477062, 0.00446551, -0.0012799, 0.00167925, 0.000267742, -0.000903358, -0.0116068, -0.0399685, -0.000686781, -0.0111621, -0.00392442, 0.00435219, 0.00613091, -0.00343162, -0.00873929, -0.00498964], - [0.00698102, 0.17203, -0.135309, -0.0530585, -0.0274505, -0.0986184, 0.0199336, -0.0157709, -0.0144523, -0.0057738, 0.225099, 0.0442009, 0.0109938, 0.0135212, 0.130972, 0.00595479, -0.0246616, 0.0116247, 0.0376314, 0.00219893, -0.17391, -0.0377317, 0.00496186, 0.00381288, 0.0101307, -0.00426805, 0.00579589, -0.0141228, 0.0269361, -0.00380837, 0.000902046, -0.00042646, -0.00801859, 0.020572, 0.0039739, -0.00696142, 0.0276635, 0.0476737, -0.00153598, 0.00592851, -0.0216158, -0.014092, -0.00735922, 0.00370974, -0.0538922, 0.00122146, -0.0433639, -0.00608319, -0.00581932, -0.0254123, 0.0199356, 0.0106324, 0.00251351, 0.0192596, 0.0220464, 0.0151556, -0.0099076, -0.00955052, -0.00566445, 0.00436284, 0.0244606, -0.00943798, 0.013099, -0.00128335, 9.90331e-05, -0.0107958, -0.00261728, 0.00379955, -0.00506443, 0.0135961, 0.0105554, -0.00377641, -0.00402787, -0.000316862, 0.00159515, 0.00603265, 0.00461063, 0.00259039, -0.00104727, -0.00086985, -0.00191415, -0.000168667, 0.00097723, -0.00154505, 0.00751481, -0.00186548, 0.00124077, 0.000453952, -0.000606508, 0.00533578, 0.00218407, 0.00453982, 0.00082932, 0.00100676, 0.00185951, -0.0013692, 0.000262502, -0.00179054, 0.00876282, 0.0492979, 0.0125155, -0.00136387, -0.000319577, -0.011553, 0.0106951, 0.00197776, -0.000175277, -0.000175856, -0.00428024, -0.00248809, 0.000817511, 0.0118462, 0.00909968, -0.001888, -0.0199842, -0.0197348, 0.00597335, 0.00265272, -0.000482235, -0.00247393, -0.0019193, 0.00232815, 0.00360518, -0.00251869, -0.00537591, -0.00307388, -0.00700011, 0.00522888, 0.000153726, 0.00022249, -0.00224076, -0.00474255, -0.000131328, -0.000530219, 0.000999337, -0.00765933, 0.00748947, 0.00385722, 0.00053268, 0.00158917, -0.000789119, 0.00316381, -0.000937859, 4.05151e-05, -0.0294117, -0.0818586, -0.153856, -0.0288832, 0.00859953, 0.0104256, -0.0222873, 0.000147819, 0.00422314, -0.00698901, 0.0560896, -0.00814602, -0.00126066, -0.0382772, 0.00978097, 0.00476111, -0.00192896, 0.00138786, 0.00727908, 0.00629046, 0.0205368, -0.0193913, -0.00771905, -0.00555957, -0.000546143, -0.000160415, 0.00851434, -0.00715185, -0.00163088, -0.000876829, 0.000225337, 0.00403854, -0.00366158, -0.000243572, 0.000672828, 0.00148787, 0.0589061, 0.00907169, 0.00627746, -0.00714632, -0.00638265, -0.00791136, 0.0035721, -0.00164952, 0.00134629]] */ + /* #1: @1 = + [[-0.0627168, 0.00911909, 0.113161, 0.0429596, 0.0215499, -0.0190236, + 0.00690652, -0.00189939, 0.002902, 0.0607266, -0.00212102, 0.12994, 0.0448556, + 0.0249316, -0.00230252, -0.00362391, 0.0010149, 0.00561049, 0.00738982, + 0.0688731, 0.00180948, 0.00582719, 0.00732466, -0.000265723, 0.00317307, + -0.000477502, 0.000614032, -0.000308676, 0.00728648, 0.00378877, -0.000788835, + -0.00171585, -0.000390278, 0.00067807, 8.72168e-05, -0.000138241, 0.000108059, + 0.0157792, -0.000739303, 0.00578389, 0.00424382, 0.00213685, 0.00126477, + 0.00206954, 0.00615703, 0.00169051, -0.0097154, -0.000728647, -0.00357184, + -0.00681516, 0.00484937, 0.00155446, -2.22236e-05, -8.87748e-05, -0.0132386, + -0.0103304, -0.00186689, -0.0010376, -0.000515239, -0.00050907, -0.00123645, + 0.00840625, 0.00571376, 0.00199724, 0.00370229, 0.0084076, 0.00237273, + 0.000642649, 0.00210247, -0.00912764, -0.00753473, -0.000724963, -0.00100813, + -0.000475941, -2.6334e-05, 0.00132596, 0.000911836, 0.00361837, 0.00198928, + 0.00132822, 0.00239418, 0.00122732, 0.000254297, 0.000857163, + -0.00594194, 3.62108e-05, -0.00021935, -0.000182416, 0.000107603, 0.00269593, + 0.00375798, 0.00114746, 0.000627986, -3.32492e-05, 0.000411532, 0.000765713, + 0.000151893, 0.000185052, -0.00903996, 0.0135454, -0.000722359, -0.00490391, + 0.00203116, 0.00710772, 0.0076969, 0.00173302, 0.00100631, 0.00420003, + 0.000574205, -0.000373772, 0.00102787, -0.00561679, -0.0023325, -0.00431477, + 0.00044308, 0.00322925, -0.00353879, -0.00194758, -0.000349419, -0.00114885, + 0.00218443, 0.000315052, -0.000246945, -0.000234972, 0.00343182, -0.000576893, + 0.00128453, -0.000627002, -0.000244513, -7.21641e-05, 0.000186332, + 0.000150758, -0.000141313, 7.20824e-05, -0.00178129, 0.000312008, 0.00118627, + 0.00151454, 0.000523475, 0.000436795, 0.00116518, 0.000607068, 3.05045e-05, + 0.000184028, 0.0117549, 0.0796893, -0.00214423, -0.00525946, -0.00403565, + 0.000691185, 0.00321344, -8.39683e-05, 0.000361989, -0.00329383, -0.010862, + -0.000101139, -0.00339473, -0.00643871, 0.00571431, 0.00230936, -0.00149976, + -0.00183948, -0.0151343, -0.0121317, -0.00300954, 0.00254861, 0.000666956, + -0.00133131, -0.0101003, -0.00841691, -0.000414823, 0.000461035, 0.00029282, + -0.000105389, -0.00673323, -0.000421961, 0.00101527, 0.000355623, + -9.10409e-05, -0.00796872, -0.00509959, 0.000959568, -0.00373789, -0.00553031, + -0.00328613, 0.00408343, 0.000243895, -0.00241938, -0.00116566], [0.0186502, + -0.0152338, 0.113699, 0.0327646, 0.020499, 0.108095, 0.00313986, 0.00353094, + 0.0219446, 0.0203845, -0.0201273, -0.107101, -0.027541, -0.0184919, -0.11181, + 0.00255492, 0.00226364, -0.0213281, 0.00436631, 0.0782532, -0.0103053, + -0.00250494, -0.00615842, 0.000778518, -0.00575801, -0.000519435, -0.00156735, + 0.00318815, -0.000309406, 0.00563101, 0.000204443, 5.68998e-05, -0.000929549, + -0.00403978, -0.00091885, 0.000161048, -0.00316028, -0.0334966, 0.00850775, + -0.0130498, -0.000829289, 0.00390585, 0.00226736, -0.00122417, 0.00455176, + 0.00103396, 0.0175465, 0.000130719, 0.00269078, 0.00230771, -0.00480044, + -0.000810291, -0.000815402, 0.00352587, -0.0104141, -0.00833722, -0.005023, + 0.00403064, 0.00142261, -0.00163339, -0.00459653, 0.0153418, -0.00608615, + -0.00163041, 0.00151932, -0.00262283, 0.0138738, 0.00181605, -0.00365686, + -0.00782621, -0.00683235, -0.000510232, 0.0017275, 0.000173605, 0.000342975, + -0.00261291, -0.00100338, -0.00121781, -0.00026139, -4.05719e-05, -0.00145568, + 0.0055267, 0.000959155, -0.00107796, -0.00583856, 0.00117568, 0.00183215, + 0.000408175, 0.00088, -0.000102079, 0.00278011, -0.00347983, -0.000377072, + -0.000156108, -0.00110755, 0.00160366, 7.10275e-05, -0.000213424, 0.0112871, + -0.0382876, 1.51803e-05, 0.0107112, -0.00273886, 0.00238821, 0.000870183, + 0.000674078, 0.000674308, 9.9041e-05, 0.0010109, 0.000694078, -0.000421943, + 0.00352808, -0.00459972, 0.0054003, 0.0056865, 0.00739394, 0.014655, + 0.00521551, 0.00175918, 0.00134017, -4.93111e-05, 0.000393203, -0.000179556, + 0.00324963, -0.00830727, 0.00117472, 0.00104853, 0.00224924, 0.00096319, + 0.000193481, 0.000932703, -0.00048353, -1.45417e-05, -0.000183356, 0.00493695, + -0.00059341, -0.00117813, 0.00318192, 0.000904924, 9.0911e-06, 0.000364803, + -0.00028243, -0.000141499, -4.08834e-05, -0.00241024, -0.062841, -0.0194669, + -0.00606772, -0.00765537, 0.000140633, 0.0063877, -0.000249763, -0.00191831, + 0.00376216, -0.034627, 0.00946453, -0.0129204, -0.000289844, -0.00292233, + -0.00266214, -0.0014625, -0.0102087, 0.0042977, 0.00396114, 0.0021332, + 0.00494168, 0.00123231, 0.000931988, 0.00571952, 0.00509258, -0.00024296, + 0.00221941, 0.000214861, -0.000477062, 0.00446551, -0.0012799, 0.00167925, + 0.000267742, -0.000903358, -0.0116068, -0.0399685, -0.000686781, -0.0111621, + -0.00392442, 0.00435219, 0.00613091, -0.00343162, -0.00873929, -0.00498964], + [0.00698102, 0.17203, -0.135309, -0.0530585, -0.0274505, -0.0986184, + 0.0199336, -0.0157709, -0.0144523, -0.0057738, 0.225099, 0.0442009, 0.0109938, + 0.0135212, 0.130972, 0.00595479, -0.0246616, 0.0116247, 0.0376314, 0.00219893, + -0.17391, -0.0377317, 0.00496186, 0.00381288, 0.0101307, -0.00426805, + 0.00579589, -0.0141228, 0.0269361, -0.00380837, 0.000902046, -0.00042646, + -0.00801859, 0.020572, 0.0039739, -0.00696142, 0.0276635, 0.0476737, + -0.00153598, 0.00592851, -0.0216158, -0.014092, -0.00735922, 0.00370974, + -0.0538922, 0.00122146, -0.0433639, -0.00608319, -0.00581932, -0.0254123, + 0.0199356, 0.0106324, 0.00251351, 0.0192596, 0.0220464, 0.0151556, -0.0099076, + -0.00955052, -0.00566445, 0.00436284, 0.0244606, -0.00943798, 0.013099, + -0.00128335, 9.90331e-05, -0.0107958, -0.00261728, 0.00379955, -0.00506443, + 0.0135961, 0.0105554, -0.00377641, -0.00402787, -0.000316862, 0.00159515, + 0.00603265, 0.00461063, 0.00259039, -0.00104727, -0.00086985, -0.00191415, + -0.000168667, 0.00097723, -0.00154505, 0.00751481, -0.00186548, 0.00124077, + 0.000453952, -0.000606508, 0.00533578, 0.00218407, 0.00453982, 0.00082932, + 0.00100676, 0.00185951, -0.0013692, 0.000262502, -0.00179054, 0.00876282, + 0.0492979, 0.0125155, -0.00136387, -0.000319577, -0.011553, 0.0106951, + 0.00197776, -0.000175277, -0.000175856, -0.00428024, -0.00248809, 0.000817511, + 0.0118462, 0.00909968, -0.001888, -0.0199842, -0.0197348, 0.00597335, + 0.00265272, -0.000482235, -0.00247393, -0.0019193, 0.00232815, 0.00360518, + -0.00251869, -0.00537591, -0.00307388, -0.00700011, 0.00522888, 0.000153726, + 0.00022249, -0.00224076, -0.00474255, -0.000131328, -0.000530219, 0.000999337, + -0.00765933, 0.00748947, 0.00385722, 0.00053268, 0.00158917, -0.000789119, + 0.00316381, -0.000937859, 4.05151e-05, -0.0294117, -0.0818586, -0.153856, + -0.0288832, 0.00859953, 0.0104256, -0.0222873, 0.000147819, 0.00422314, + -0.00698901, 0.0560896, -0.00814602, -0.00126066, -0.0382772, 0.00978097, + 0.00476111, -0.00192896, 0.00138786, 0.00727908, 0.00629046, 0.0205368, + -0.0193913, -0.00771905, -0.00555957, -0.000546143, -0.000160415, 0.00851434, + -0.00715185, -0.00163088, -0.000876829, 0.000225337, 0.00403854, -0.00366158, + -0.000243572, 0.000672828, 0.00148787, 0.0589061, 0.00907169, 0.00627746, + -0.00714632, -0.00638265, -0.00791136, 0.0035721, -0.00164952, 0.00134629]] */ casadi_copy(casadi_c0, 567, w1); /* #2: @2 = input[0][0] */ casadi_copy(arg[0], 18, w2); /* #3: @3 = @2[0] */ - for (rr=(&w3), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w3), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #4: @4 = @2[1] */ - for (rr=(&w4), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w4), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #5: @5 = @2[2] */ - for (rr=(&w5), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w5), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #6: @6 = @2[3] */ - for (rr=(&w6), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w6), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #7: @7 = @2[4] */ - for (rr=(&w7), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #8: @8 = @2[5] */ - for (rr=(&w8), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w8), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #9: @9 = @2[6] */ - for (rr=(&w9), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w9), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #10: @10 = @2[7] */ - for (rr=(&w10), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w10), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #11: @11 = @2[8] */ - for (rr=(&w11), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w11), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #12: @12 = @2[9] */ - for (rr=(&w12), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w12), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #13: @13 = @2[10] */ - for (rr=(&w13), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w13), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #14: @14 = @2[11] */ - for (rr=(&w14), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w14), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #15: @15 = @2[12] */ - for (rr=(&w15), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w15), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #16: @16 = @2[13] */ - for (rr=(&w16), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w16), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #17: @17 = @2[14] */ - for (rr=(&w17), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w17), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #18: @18 = @2[15] */ - for (rr=(&w18), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w18), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #19: @19 = @2[16] */ - for (rr=(&w19), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w19), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #20: @20 = @2[17] */ - for (rr=(&w20), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w20), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #21: @21 = @2[0] */ - for (rr=(&w21), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w21), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #22: @21 = sq(@21) */ - w21 = casadi_sq( w21 ); + w21 = casadi_sq(w21); /* #23: @22 = @2[0] */ - for (rr=(&w22), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w22), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #24: @23 = @2[1] */ - for (rr=(&w23), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w23), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #25: @22 = (@22*@23) */ w22 *= w23; /* #26: @23 = @2[0] */ - for (rr=(&w23), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w23), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #27: @24 = @2[2] */ - for (rr=(&w24), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w24), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #28: @23 = (@23*@24) */ w23 *= w24; /* #29: @24 = @2[0] */ - for (rr=(&w24), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w24), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #30: @25 = @2[3] */ - for (rr=(&w25), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w25), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #31: @24 = (@24*@25) */ w24 *= w25; /* #32: @25 = @2[0] */ - for (rr=(&w25), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w25), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #33: @26 = @2[4] */ - for (rr=(&w26), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w26), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #34: @25 = (@25*@26) */ w25 *= w26; /* #35: @26 = @2[0] */ - for (rr=(&w26), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w26), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #36: @27 = @2[5] */ - for (rr=(&w27), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w27), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #37: @26 = (@26*@27) */ w26 *= w27; /* #38: @27 = @2[0] */ - for (rr=(&w27), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w27), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #39: @28 = @2[6] */ - for (rr=(&w28), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w28), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #40: @27 = (@27*@28) */ w27 *= w28; /* #41: @28 = @2[0] */ - for (rr=(&w28), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w28), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #42: @29 = @2[7] */ - for (rr=(&w29), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w29), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #43: @28 = (@28*@29) */ w28 *= w29; /* #44: @29 = @2[0] */ - for (rr=(&w29), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w29), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #45: @30 = @2[8] */ - for (rr=(&w30), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w30), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #46: @29 = (@29*@30) */ w29 *= w30; /* #47: @30 = @2[0] */ - for (rr=(&w30), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w30), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #48: @31 = @2[9] */ - for (rr=(&w31), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w31), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #49: @30 = (@30*@31) */ w30 *= w31; /* #50: @31 = @2[0] */ - for (rr=(&w31), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w31), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #51: @32 = @2[10] */ - for (rr=(&w32), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w32), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #52: @31 = (@31*@32) */ w31 *= w32; /* #53: @32 = @2[0] */ - for (rr=(&w32), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w32), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #54: @33 = @2[11] */ - for (rr=(&w33), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w33), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #55: @32 = (@32*@33) */ w32 *= w33; /* #56: @33 = @2[0] */ - for (rr=(&w33), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w33), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #57: @34 = @2[12] */ - for (rr=(&w34), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w34), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #58: @33 = (@33*@34) */ w33 *= w34; /* #59: @34 = @2[0] */ - for (rr=(&w34), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w34), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #60: @35 = @2[13] */ - for (rr=(&w35), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w35), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #61: @34 = (@34*@35) */ w34 *= w35; /* #62: @35 = @2[0] */ - for (rr=(&w35), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w35), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #63: @36 = @2[14] */ - for (rr=(&w36), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w36), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #64: @35 = (@35*@36) */ w35 *= w36; /* #65: @36 = @2[0] */ - for (rr=(&w36), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w36), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #66: @37 = @2[15] */ - for (rr=(&w37), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w37), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #67: @36 = (@36*@37) */ w36 *= w37; /* #68: @37 = @2[0] */ - for (rr=(&w37), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w37), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #69: @38 = @2[16] */ - for (rr=(&w38), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w38), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #70: @37 = (@37*@38) */ w37 *= w38; /* #71: @38 = @2[0] */ - for (rr=(&w38), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w38), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #72: @39 = @2[17] */ - for (rr=(&w39), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w39), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #73: @38 = (@38*@39) */ w38 *= w39; /* #74: @39 = @2[1] */ - for (rr=(&w39), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w39), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #75: @39 = sq(@39) */ - w39 = casadi_sq( w39 ); + w39 = casadi_sq(w39); /* #76: @40 = @2[1] */ - for (rr=(&w40), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w40), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #77: @41 = @2[2] */ - for (rr=(&w41), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w41), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #78: @40 = (@40*@41) */ w40 *= w41; /* #79: @41 = @2[1] */ - for (rr=(&w41), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w41), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #80: @42 = @2[3] */ - for (rr=(&w42), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w42), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #81: @41 = (@41*@42) */ w41 *= w42; /* #82: @42 = @2[1] */ - for (rr=(&w42), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w42), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #83: @43 = @2[4] */ - for (rr=(&w43), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w43), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #84: @42 = (@42*@43) */ w42 *= w43; /* #85: @43 = @2[1] */ - for (rr=(&w43), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w43), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #86: @44 = @2[5] */ - for (rr=(&w44), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w44), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #87: @43 = (@43*@44) */ w43 *= w44; /* #88: @44 = @2[1] */ - for (rr=(&w44), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w44), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #89: @45 = @2[6] */ - for (rr=(&w45), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w45), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #90: @44 = (@44*@45) */ w44 *= w45; /* #91: @45 = @2[1] */ - for (rr=(&w45), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w45), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #92: @46 = @2[7] */ - for (rr=(&w46), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w46), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #93: @45 = (@45*@46) */ w45 *= w46; /* #94: @46 = @2[1] */ - for (rr=(&w46), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w46), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #95: @47 = @2[8] */ - for (rr=(&w47), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w47), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #96: @46 = (@46*@47) */ w46 *= w47; /* #97: @47 = @2[1] */ - for (rr=(&w47), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w47), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #98: @48 = @2[9] */ - for (rr=(&w48), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w48), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #99: @47 = (@47*@48) */ w47 *= w48; /* #100: @48 = @2[1] */ - for (rr=(&w48), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w48), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #101: @49 = @2[10] */ - for (rr=(&w49), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w49), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #102: @48 = (@48*@49) */ w48 *= w49; /* #103: @49 = @2[1] */ - for (rr=(&w49), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w49), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #104: @50 = @2[11] */ - for (rr=(&w50), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w50), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #105: @49 = (@49*@50) */ w49 *= w50; /* #106: @50 = @2[1] */ - for (rr=(&w50), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w50), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #107: @51 = @2[12] */ - for (rr=(&w51), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w51), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #108: @50 = (@50*@51) */ w50 *= w51; /* #109: @51 = @2[1] */ - for (rr=(&w51), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w51), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #110: @52 = @2[13] */ - for (rr=(&w52), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w52), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #111: @51 = (@51*@52) */ w51 *= w52; /* #112: @52 = @2[1] */ - for (rr=(&w52), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w52), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #113: @53 = @2[14] */ - for (rr=(&w53), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w53), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #114: @52 = (@52*@53) */ w52 *= w53; /* #115: @53 = @2[1] */ - for (rr=(&w53), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w53), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #116: @54 = @2[15] */ - for (rr=(&w54), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w54), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #117: @53 = (@53*@54) */ w53 *= w54; /* #118: @54 = @2[1] */ - for (rr=(&w54), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w54), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #119: @55 = @2[16] */ - for (rr=(&w55), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w55), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #120: @54 = (@54*@55) */ w54 *= w55; /* #121: @55 = @2[1] */ - for (rr=(&w55), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w55), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #122: @56 = @2[17] */ - for (rr=(&w56), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w56), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #123: @55 = (@55*@56) */ w55 *= w56; /* #124: @56 = @2[2] */ - for (rr=(&w56), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w56), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #125: @56 = sq(@56) */ - w56 = casadi_sq( w56 ); + w56 = casadi_sq(w56); /* #126: @57 = @2[2] */ - for (rr=(&w57), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w57), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #127: @58 = @2[3] */ - for (rr=(&w58), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w58), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #128: @57 = (@57*@58) */ w57 *= w58; /* #129: @58 = @2[2] */ - for (rr=(&w58), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w58), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #130: @59 = @2[4] */ - for (rr=(&w59), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w59), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #131: @58 = (@58*@59) */ w58 *= w59; /* #132: @59 = @2[2] */ - for (rr=(&w59), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w59), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #133: @60 = @2[5] */ - for (rr=(&w60), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w60), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #134: @59 = (@59*@60) */ w59 *= w60; /* #135: @60 = @2[2] */ - for (rr=(&w60), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w60), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #136: @61 = @2[6] */ - for (rr=(&w61), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w61), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #137: @60 = (@60*@61) */ w60 *= w61; /* #138: @61 = @2[2] */ - for (rr=(&w61), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w61), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #139: @62 = @2[7] */ - for (rr=(&w62), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w62), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #140: @61 = (@61*@62) */ w61 *= w62; /* #141: @62 = @2[2] */ - for (rr=(&w62), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w62), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #142: @63 = @2[8] */ - for (rr=(&w63), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w63), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #143: @62 = (@62*@63) */ w62 *= w63; /* #144: @63 = @2[2] */ - for (rr=(&w63), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w63), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #145: @64 = @2[9] */ - for (rr=(&w64), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w64), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #146: @63 = (@63*@64) */ w63 *= w64; /* #147: @64 = @2[2] */ - for (rr=(&w64), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w64), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #148: @65 = @2[10] */ - for (rr=(&w65), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w65), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #149: @64 = (@64*@65) */ w64 *= w65; /* #150: @65 = @2[2] */ - for (rr=(&w65), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w65), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #151: @66 = @2[11] */ - for (rr=(&w66), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w66), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #152: @65 = (@65*@66) */ w65 *= w66; /* #153: @66 = @2[2] */ - for (rr=(&w66), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w66), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #154: @67 = @2[12] */ - for (rr=(&w67), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w67), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #155: @66 = (@66*@67) */ w66 *= w67; /* #156: @67 = @2[2] */ - for (rr=(&w67), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w67), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #157: @68 = @2[13] */ - for (rr=(&w68), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w68), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #158: @67 = (@67*@68) */ w67 *= w68; /* #159: @68 = @2[2] */ - for (rr=(&w68), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w68), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #160: @69 = @2[14] */ - for (rr=(&w69), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w69), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #161: @68 = (@68*@69) */ w68 *= w69; /* #162: @69 = @2[2] */ - for (rr=(&w69), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w69), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #163: @70 = @2[15] */ - for (rr=(&w70), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w70), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #164: @69 = (@69*@70) */ w69 *= w70; /* #165: @70 = @2[2] */ - for (rr=(&w70), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w70), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #166: @71 = @2[16] */ - for (rr=(&w71), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w71), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #167: @70 = (@70*@71) */ w70 *= w71; /* #168: @71 = @2[2] */ - for (rr=(&w71), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w71), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #169: @72 = @2[17] */ - for (rr=(&w72), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w72), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #170: @71 = (@71*@72) */ w71 *= w72; /* #171: @72 = @2[3] */ - for (rr=(&w72), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w72), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #172: @72 = sq(@72) */ - w72 = casadi_sq( w72 ); + w72 = casadi_sq(w72); /* #173: @73 = @2[3] */ - for (rr=(&w73), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w73), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #174: @74 = @2[4] */ - for (rr=(&w74), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w74), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #175: @73 = (@73*@74) */ w73 *= w74; /* #176: @74 = @2[3] */ - for (rr=(&w74), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w74), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #177: @75 = @2[5] */ - for (rr=(&w75), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w75), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #178: @74 = (@74*@75) */ w74 *= w75; /* #179: @75 = @2[3] */ - for (rr=(&w75), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w75), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #180: @76 = @2[6] */ - for (rr=(&w76), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w76), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #181: @75 = (@75*@76) */ w75 *= w76; /* #182: @76 = @2[3] */ - for (rr=(&w76), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w76), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #183: @77 = @2[7] */ - for (rr=(&w77), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w77), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #184: @76 = (@76*@77) */ w76 *= w77; /* #185: @77 = @2[3] */ - for (rr=(&w77), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w77), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #186: @78 = @2[8] */ - for (rr=(&w78), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w78), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #187: @77 = (@77*@78) */ w77 *= w78; /* #188: @78 = @2[3] */ - for (rr=(&w78), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w78), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #189: @79 = @2[9] */ - for (rr=(&w79), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w79), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #190: @78 = (@78*@79) */ w78 *= w79; /* #191: @79 = @2[3] */ - for (rr=(&w79), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w79), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #192: @80 = @2[10] */ - for (rr=(&w80), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w80), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #193: @79 = (@79*@80) */ w79 *= w80; /* #194: @80 = @2[3] */ - for (rr=(&w80), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w80), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #195: @81 = @2[11] */ - for (rr=(&w81), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w81), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #196: @80 = (@80*@81) */ w80 *= w81; /* #197: @81 = @2[3] */ - for (rr=(&w81), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w81), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #198: @82 = @2[12] */ - for (rr=(&w82), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w82), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #199: @81 = (@81*@82) */ w81 *= w82; /* #200: @82 = @2[3] */ - for (rr=(&w82), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w82), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #201: @83 = @2[13] */ - for (rr=(&w83), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w83), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #202: @82 = (@82*@83) */ w82 *= w83; /* #203: @83 = @2[3] */ - for (rr=(&w83), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w83), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #204: @84 = @2[14] */ - for (rr=(&w84), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w84), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #205: @83 = (@83*@84) */ w83 *= w84; /* #206: @84 = @2[3] */ - for (rr=(&w84), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w84), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #207: @85 = @2[15] */ - for (rr=(&w85), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w85), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #208: @84 = (@84*@85) */ w84 *= w85; /* #209: @85 = @2[3] */ - for (rr=(&w85), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w85), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #210: @86 = @2[16] */ - for (rr=(&w86), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w86), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #211: @85 = (@85*@86) */ w85 *= w86; /* #212: @86 = @2[3] */ - for (rr=(&w86), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w86), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #213: @87 = @2[17] */ - for (rr=(&w87), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w87), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #214: @86 = (@86*@87) */ w86 *= w87; /* #215: @87 = @2[4] */ - for (rr=(&w87), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w87), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #216: @87 = sq(@87) */ - w87 = casadi_sq( w87 ); + w87 = casadi_sq(w87); /* #217: @88 = @2[4] */ - for (rr=(&w88), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w88), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #218: @89 = @2[5] */ - for (rr=(&w89), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w89), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #219: @88 = (@88*@89) */ w88 *= w89; /* #220: @89 = @2[4] */ - for (rr=(&w89), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w89), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #221: @90 = @2[6] */ - for (rr=(&w90), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w90), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #222: @89 = (@89*@90) */ w89 *= w90; /* #223: @90 = @2[4] */ - for (rr=(&w90), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w90), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #224: @91 = @2[7] */ - for (rr=(&w91), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w91), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #225: @90 = (@90*@91) */ w90 *= w91; /* #226: @91 = @2[4] */ - for (rr=(&w91), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w91), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #227: @92 = @2[8] */ - for (rr=(&w92), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w92), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #228: @91 = (@91*@92) */ w91 *= w92; /* #229: @92 = @2[4] */ - for (rr=(&w92), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w92), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #230: @93 = @2[9] */ - for (rr=(&w93), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w93), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #231: @92 = (@92*@93) */ w92 *= w93; /* #232: @93 = @2[4] */ - for (rr=(&w93), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w93), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #233: @94 = @2[10] */ - for (rr=(&w94), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w94), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #234: @93 = (@93*@94) */ w93 *= w94; /* #235: @94 = @2[4] */ - for (rr=(&w94), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w94), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #236: @95 = @2[11] */ - for (rr=(&w95), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w95), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #237: @94 = (@94*@95) */ w94 *= w95; /* #238: @95 = @2[4] */ - for (rr=(&w95), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w95), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #239: @96 = @2[12] */ - for (rr=(&w96), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w96), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #240: @95 = (@95*@96) */ w95 *= w96; /* #241: @96 = @2[4] */ - for (rr=(&w96), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w96), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #242: @97 = @2[13] */ - for (rr=(&w97), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w97), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #243: @96 = (@96*@97) */ w96 *= w97; /* #244: @97 = @2[4] */ - for (rr=(&w97), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w97), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #245: @98 = @2[14] */ - for (rr=(&w98), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w98), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #246: @97 = (@97*@98) */ w97 *= w98; /* #247: @98 = @2[4] */ - for (rr=(&w98), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w98), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #248: @99 = @2[15] */ - for (rr=(&w99), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w99), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #249: @98 = (@98*@99) */ w98 *= w99; /* #250: @99 = @2[4] */ - for (rr=(&w99), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w99), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #251: @100 = @2[16] */ - for (rr=(&w100), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w100), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #252: @99 = (@99*@100) */ w99 *= w100; /* #253: @100 = @2[4] */ - for (rr=(&w100), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w100), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #254: @101 = @2[17] */ - for (rr=(&w101), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w101), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #255: @100 = (@100*@101) */ w100 *= w101; /* #256: @101 = @2[5] */ - for (rr=(&w101), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w101), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #257: @101 = sq(@101) */ - w101 = casadi_sq( w101 ); + w101 = casadi_sq(w101); /* #258: @102 = @2[5] */ - for (rr=(&w102), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w102), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #259: @103 = @2[6] */ - for (rr=(&w103), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w103), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #260: @102 = (@102*@103) */ w102 *= w103; /* #261: @103 = @2[5] */ - for (rr=(&w103), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w103), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #262: @104 = @2[7] */ - for (rr=(&w104), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w104), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #263: @103 = (@103*@104) */ w103 *= w104; /* #264: @104 = @2[5] */ - for (rr=(&w104), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w104), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #265: @105 = @2[8] */ - for (rr=(&w105), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w105), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #266: @104 = (@104*@105) */ w104 *= w105; /* #267: @105 = @2[5] */ - for (rr=(&w105), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w105), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #268: @106 = @2[9] */ - for (rr=(&w106), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w106), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #269: @105 = (@105*@106) */ w105 *= w106; /* #270: @106 = @2[5] */ - for (rr=(&w106), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w106), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #271: @107 = @2[10] */ - for (rr=(&w107), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w107), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #272: @106 = (@106*@107) */ w106 *= w107; /* #273: @107 = @2[5] */ - for (rr=(&w107), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w107), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #274: @108 = @2[11] */ - for (rr=(&w108), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w108), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #275: @107 = (@107*@108) */ w107 *= w108; /* #276: @108 = @2[5] */ - for (rr=(&w108), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w108), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #277: @109 = @2[12] */ - for (rr=(&w109), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w109), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #278: @108 = (@108*@109) */ w108 *= w109; /* #279: @109 = @2[5] */ - for (rr=(&w109), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w109), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #280: @110 = @2[13] */ - for (rr=(&w110), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w110), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #281: @109 = (@109*@110) */ w109 *= w110; /* #282: @110 = @2[5] */ - for (rr=(&w110), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w110), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #283: @111 = @2[14] */ - for (rr=(&w111), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w111), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #284: @110 = (@110*@111) */ w110 *= w111; /* #285: @111 = @2[5] */ - for (rr=(&w111), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w111), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #286: @112 = @2[15] */ - for (rr=(&w112), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w112), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #287: @111 = (@111*@112) */ w111 *= w112; /* #288: @112 = @2[5] */ - for (rr=(&w112), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w112), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #289: @113 = @2[16] */ - for (rr=(&w113), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w113), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #290: @112 = (@112*@113) */ w112 *= w113; /* #291: @113 = @2[5] */ - for (rr=(&w113), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w113), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #292: @114 = @2[17] */ - for (rr=(&w114), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w114), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #293: @113 = (@113*@114) */ w113 *= w114; /* #294: @114 = @2[6] */ - for (rr=(&w114), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w114), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #295: @114 = sq(@114) */ - w114 = casadi_sq( w114 ); + w114 = casadi_sq(w114); /* #296: @115 = @2[6] */ - for (rr=(&w115), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w115), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #297: @116 = @2[7] */ - for (rr=(&w116), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w116), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #298: @115 = (@115*@116) */ w115 *= w116; /* #299: @116 = @2[6] */ - for (rr=(&w116), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w116), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #300: @117 = @2[8] */ - for (rr=(&w117), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w117), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #301: @116 = (@116*@117) */ w116 *= w117; /* #302: @117 = @2[6] */ - for (rr=(&w117), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w117), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #303: @118 = @2[9] */ - for (rr=(&w118), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w118), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #304: @117 = (@117*@118) */ w117 *= w118; /* #305: @118 = @2[6] */ - for (rr=(&w118), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w118), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #306: @119 = @2[10] */ - for (rr=(&w119), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w119), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #307: @118 = (@118*@119) */ w118 *= w119; /* #308: @119 = @2[6] */ - for (rr=(&w119), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w119), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #309: @120 = @2[11] */ - for (rr=(&w120), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w120), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #310: @119 = (@119*@120) */ w119 *= w120; /* #311: @120 = @2[6] */ - for (rr=(&w120), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w120), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #312: @121 = @2[12] */ - for (rr=(&w121), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w121), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #313: @120 = (@120*@121) */ w120 *= w121; /* #314: @121 = @2[6] */ - for (rr=(&w121), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w121), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #315: @122 = @2[13] */ - for (rr=(&w122), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w122), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #316: @121 = (@121*@122) */ w121 *= w122; /* #317: @122 = @2[6] */ - for (rr=(&w122), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w122), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #318: @123 = @2[14] */ - for (rr=(&w123), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w123), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #319: @122 = (@122*@123) */ w122 *= w123; /* #320: @123 = @2[6] */ - for (rr=(&w123), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w123), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #321: @124 = @2[15] */ - for (rr=(&w124), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w124), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #322: @123 = (@123*@124) */ w123 *= w124; /* #323: @124 = @2[6] */ - for (rr=(&w124), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w124), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #324: @125 = @2[16] */ - for (rr=(&w125), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w125), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #325: @124 = (@124*@125) */ w124 *= w125; /* #326: @125 = @2[6] */ - for (rr=(&w125), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w125), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #327: @126 = @2[17] */ - for (rr=(&w126), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w126), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #328: @125 = (@125*@126) */ w125 *= w126; /* #329: @126 = @2[7] */ - for (rr=(&w126), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w126), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #330: @126 = sq(@126) */ - w126 = casadi_sq( w126 ); + w126 = casadi_sq(w126); /* #331: @127 = @2[7] */ - for (rr=(&w127), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w127), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #332: @128 = @2[8] */ - for (rr=(&w128), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w128), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #333: @127 = (@127*@128) */ w127 *= w128; /* #334: @128 = @2[7] */ - for (rr=(&w128), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w128), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #335: @129 = @2[9] */ - for (rr=(&w129), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w129), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #336: @128 = (@128*@129) */ w128 *= w129; /* #337: @129 = @2[7] */ - for (rr=(&w129), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w129), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #338: @130 = @2[10] */ - for (rr=(&w130), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w130), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #339: @129 = (@129*@130) */ w129 *= w130; /* #340: @130 = @2[7] */ - for (rr=(&w130), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w130), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #341: @131 = @2[11] */ - for (rr=(&w131), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w131), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #342: @130 = (@130*@131) */ w130 *= w131; /* #343: @131 = @2[7] */ - for (rr=(&w131), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w131), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #344: @132 = @2[12] */ - for (rr=(&w132), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w132), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #345: @131 = (@131*@132) */ w131 *= w132; /* #346: @132 = @2[7] */ - for (rr=(&w132), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w132), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #347: @133 = @2[13] */ - for (rr=(&w133), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w133), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #348: @132 = (@132*@133) */ w132 *= w133; /* #349: @133 = @2[7] */ - for (rr=(&w133), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w133), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #350: @134 = @2[14] */ - for (rr=(&w134), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w134), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #351: @133 = (@133*@134) */ w133 *= w134; /* #352: @134 = @2[7] */ - for (rr=(&w134), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w134), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #353: @135 = @2[15] */ - for (rr=(&w135), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w135), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #354: @134 = (@134*@135) */ w134 *= w135; /* #355: @135 = @2[7] */ - for (rr=(&w135), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w135), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #356: @136 = @2[16] */ - for (rr=(&w136), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w136), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #357: @135 = (@135*@136) */ w135 *= w136; /* #358: @136 = @2[7] */ - for (rr=(&w136), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w136), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #359: @137 = @2[17] */ - for (rr=(&w137), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w137), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #360: @136 = (@136*@137) */ w136 *= w137; /* #361: @137 = @2[8] */ - for (rr=(&w137), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w137), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #362: @137 = sq(@137) */ - w137 = casadi_sq( w137 ); + w137 = casadi_sq(w137); /* #363: @138 = @2[8] */ - for (rr=(&w138), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w138), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #364: @139 = @2[9] */ - for (rr=(&w139), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w139), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #365: @138 = (@138*@139) */ w138 *= w139; /* #366: @139 = @2[8] */ - for (rr=(&w139), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w139), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #367: @140 = @2[10] */ - for (rr=(&w140), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w140), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #368: @139 = (@139*@140) */ w139 *= w140; /* #369: @140 = @2[8] */ - for (rr=(&w140), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w140), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #370: @141 = @2[11] */ - for (rr=(&w141), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w141), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #371: @140 = (@140*@141) */ w140 *= w141; /* #372: @141 = @2[8] */ - for (rr=(&w141), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w141), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #373: @142 = @2[12] */ - for (rr=(&w142), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w142), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #374: @141 = (@141*@142) */ w141 *= w142; /* #375: @142 = @2[8] */ - for (rr=(&w142), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w142), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #376: @143 = @2[13] */ - for (rr=(&w143), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w143), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #377: @142 = (@142*@143) */ w142 *= w143; /* #378: @143 = @2[8] */ - for (rr=(&w143), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w143), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #379: @144 = @2[14] */ - for (rr=(&w144), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w144), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #380: @143 = (@143*@144) */ w143 *= w144; /* #381: @144 = @2[8] */ - for (rr=(&w144), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w144), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #382: @145 = @2[15] */ - for (rr=(&w145), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w145), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #383: @144 = (@144*@145) */ w144 *= w145; /* #384: @145 = @2[8] */ - for (rr=(&w145), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w145), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #385: @146 = @2[16] */ - for (rr=(&w146), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w146), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #386: @145 = (@145*@146) */ w145 *= w146; /* #387: @146 = @2[8] */ - for (rr=(&w146), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w146), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #388: @147 = @2[17] */ - for (rr=(&w147), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w147), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #389: @146 = (@146*@147) */ w146 *= w147; /* #390: @147 = @2[9] */ - for (rr=(&w147), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w147), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #391: @147 = sq(@147) */ - w147 = casadi_sq( w147 ); + w147 = casadi_sq(w147); /* #392: @148 = @2[9] */ - for (rr=(&w148), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w148), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #393: @149 = @2[10] */ - for (rr=(&w149), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w149), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #394: @148 = (@148*@149) */ w148 *= w149; /* #395: @149 = @2[9] */ - for (rr=(&w149), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w149), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #396: @150 = @2[11] */ - for (rr=(&w150), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w150), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #397: @149 = (@149*@150) */ w149 *= w150; /* #398: @150 = @2[9] */ - for (rr=(&w150), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w150), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #399: @151 = @2[12] */ - for (rr=(&w151), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w151), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #400: @150 = (@150*@151) */ w150 *= w151; /* #401: @151 = @2[9] */ - for (rr=(&w151), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w151), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #402: @152 = @2[13] */ - for (rr=(&w152), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w152), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #403: @151 = (@151*@152) */ w151 *= w152; /* #404: @152 = @2[9] */ - for (rr=(&w152), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w152), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #405: @153 = @2[14] */ - for (rr=(&w153), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w153), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #406: @152 = (@152*@153) */ w152 *= w153; /* #407: @153 = @2[9] */ - for (rr=(&w153), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w153), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #408: @154 = @2[15] */ - for (rr=(&w154), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w154), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #409: @153 = (@153*@154) */ w153 *= w154; /* #410: @154 = @2[9] */ - for (rr=(&w154), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w154), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #411: @155 = @2[16] */ - for (rr=(&w155), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w155), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #412: @154 = (@154*@155) */ w154 *= w155; /* #413: @155 = @2[9] */ - for (rr=(&w155), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w155), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #414: @156 = @2[17] */ - for (rr=(&w156), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w156), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #415: @155 = (@155*@156) */ w155 *= w156; /* #416: @156 = @2[10] */ - for (rr=(&w156), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w156), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #417: @156 = sq(@156) */ - w156 = casadi_sq( w156 ); + w156 = casadi_sq(w156); /* #418: @157 = @2[10] */ - for (rr=(&w157), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w157), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #419: @158 = @2[11] */ - for (rr=(&w158), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w158), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #420: @157 = (@157*@158) */ w157 *= w158; /* #421: @158 = @2[10] */ - for (rr=(&w158), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w158), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #422: @159 = @2[12] */ - for (rr=(&w159), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w159), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #423: @158 = (@158*@159) */ w158 *= w159; /* #424: @159 = @2[10] */ - for (rr=(&w159), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w159), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #425: @160 = @2[13] */ - for (rr=(&w160), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w160), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #426: @159 = (@159*@160) */ w159 *= w160; /* #427: @160 = @2[10] */ - for (rr=(&w160), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w160), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #428: @161 = @2[14] */ - for (rr=(&w161), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w161), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #429: @160 = (@160*@161) */ w160 *= w161; /* #430: @161 = @2[10] */ - for (rr=(&w161), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w161), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #431: @162 = @2[15] */ - for (rr=(&w162), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w162), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #432: @161 = (@161*@162) */ w161 *= w162; /* #433: @162 = @2[10] */ - for (rr=(&w162), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w162), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #434: @163 = @2[16] */ - for (rr=(&w163), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w163), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #435: @162 = (@162*@163) */ w162 *= w163; /* #436: @163 = @2[10] */ - for (rr=(&w163), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w163), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #437: @164 = @2[17] */ - for (rr=(&w164), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w164), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #438: @163 = (@163*@164) */ w163 *= w164; /* #439: @164 = @2[11] */ - for (rr=(&w164), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w164), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #440: @164 = sq(@164) */ - w164 = casadi_sq( w164 ); + w164 = casadi_sq(w164); /* #441: @165 = @2[11] */ - for (rr=(&w165), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w165), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #442: @166 = @2[12] */ - for (rr=(&w166), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w166), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #443: @165 = (@165*@166) */ w165 *= w166; /* #444: @166 = @2[11] */ - for (rr=(&w166), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w166), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #445: @167 = @2[13] */ - for (rr=(&w167), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w167), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #446: @166 = (@166*@167) */ w166 *= w167; /* #447: @167 = @2[11] */ - for (rr=(&w167), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w167), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #448: @168 = @2[14] */ - for (rr=(&w168), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w168), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #449: @167 = (@167*@168) */ w167 *= w168; /* #450: @168 = @2[11] */ - for (rr=(&w168), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w168), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #451: @169 = @2[15] */ - for (rr=(&w169), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w169), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #452: @168 = (@168*@169) */ w168 *= w169; /* #453: @169 = @2[11] */ - for (rr=(&w169), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w169), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #454: @170 = @2[16] */ - for (rr=(&w170), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w170), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #455: @169 = (@169*@170) */ w169 *= w170; /* #456: @170 = @2[11] */ - for (rr=(&w170), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w170), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #457: @171 = @2[17] */ - for (rr=(&w171), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w171), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #458: @170 = (@170*@171) */ w170 *= w171; /* #459: @171 = @2[12] */ - for (rr=(&w171), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w171), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #460: @171 = sq(@171) */ - w171 = casadi_sq( w171 ); + w171 = casadi_sq(w171); /* #461: @172 = @2[12] */ - for (rr=(&w172), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w172), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #462: @173 = @2[13] */ - for (rr=(&w173), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w173), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #463: @172 = (@172*@173) */ w172 *= w173; /* #464: @173 = @2[12] */ - for (rr=(&w173), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w173), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #465: @174 = @2[14] */ - for (rr=(&w174), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w174), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #466: @173 = (@173*@174) */ w173 *= w174; /* #467: @174 = @2[12] */ - for (rr=(&w174), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w174), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #468: @175 = @2[15] */ - for (rr=(&w175), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w175), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #469: @174 = (@174*@175) */ w174 *= w175; /* #470: @175 = @2[12] */ - for (rr=(&w175), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w175), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #471: @176 = @2[16] */ - for (rr=(&w176), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w176), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #472: @175 = (@175*@176) */ w175 *= w176; /* #473: @176 = @2[12] */ - for (rr=(&w176), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w176), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #474: @177 = @2[17] */ - for (rr=(&w177), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w177), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #475: @176 = (@176*@177) */ w176 *= w177; /* #476: @177 = @2[13] */ - for (rr=(&w177), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w177), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #477: @177 = sq(@177) */ - w177 = casadi_sq( w177 ); + w177 = casadi_sq(w177); /* #478: @178 = @2[13] */ - for (rr=(&w178), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w178), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #479: @179 = @2[14] */ - for (rr=(&w179), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w179), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #480: @178 = (@178*@179) */ w178 *= w179; /* #481: @179 = @2[13] */ - for (rr=(&w179), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w179), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #482: @180 = @2[15] */ - for (rr=(&w180), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w180), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #483: @179 = (@179*@180) */ w179 *= w180; /* #484: @180 = @2[13] */ - for (rr=(&w180), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w180), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #485: @181 = @2[16] */ - for (rr=(&w181), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w181), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #486: @180 = (@180*@181) */ w180 *= w181; /* #487: @181 = @2[13] */ - for (rr=(&w181), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w181), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #488: @182 = @2[17] */ - for (rr=(&w182), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w182), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #489: @181 = (@181*@182) */ w181 *= w182; /* #490: @182 = @2[14] */ - for (rr=(&w182), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w182), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #491: @182 = sq(@182) */ - w182 = casadi_sq( w182 ); + w182 = casadi_sq(w182); /* #492: @183 = @2[14] */ - for (rr=(&w183), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w183), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #493: @184 = @2[15] */ - for (rr=(&w184), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w184), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #494: @183 = (@183*@184) */ w183 *= w184; /* #495: @184 = @2[14] */ - for (rr=(&w184), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w184), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #496: @185 = @2[16] */ - for (rr=(&w185), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w185), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #497: @184 = (@184*@185) */ w184 *= w185; /* #498: @185 = @2[14] */ - for (rr=(&w185), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w185), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #499: @186 = @2[17] */ - for (rr=(&w186), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w186), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #500: @185 = (@185*@186) */ w185 *= w186; /* #501: @186 = @2[15] */ - for (rr=(&w186), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w186), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #502: @186 = sq(@186) */ - w186 = casadi_sq( w186 ); + w186 = casadi_sq(w186); /* #503: @187 = @2[15] */ - for (rr=(&w187), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w187), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #504: @188 = @2[16] */ - for (rr=(&w188), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w188), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #505: @187 = (@187*@188) */ w187 *= w188; /* #506: @188 = @2[15] */ - for (rr=(&w188), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w188), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #507: @189 = @2[17] */ - for (rr=(&w189), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w189), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #508: @188 = (@188*@189) */ w188 *= w189; /* #509: @189 = @2[16] */ - for (rr=(&w189), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w189), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #510: @189 = sq(@189) */ - w189 = casadi_sq( w189 ); + w189 = casadi_sq(w189); /* #511: @190 = @2[16] */ - for (rr=(&w190), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w190), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #512: @191 = @2[17] */ - for (rr=(&w191), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w191), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #513: @190 = (@190*@191) */ w190 *= w191; /* #514: @191 = @2[17] */ - for (rr=(&w191), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w191), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #515: @191 = sq(@191) */ - w191 = casadi_sq( w191 ); - /* #516: @192 = vertcat(@3, @4, @5, @6, @7, @8, @9, @10, @11, @12, @13, @14, @15, @16, @17, @18, @19, @20, @21, @22, @23, @24, @25, @26, @27, @28, @29, @30, @31, @32, @33, @34, @35, @36, @37, @38, @39, @40, @41, @42, @43, @44, @45, @46, @47, @48, @49, @50, @51, @52, @53, @54, @55, @56, @57, @58, @59, @60, @61, @62, @63, @64, @65, @66, @67, @68, @69, @70, @71, @72, @73, @74, @75, @76, @77, @78, @79, @80, @81, @82, @83, @84, @85, @86, @87, @88, @89, @90, @91, @92, @93, @94, @95, @96, @97, @98, @99, @100, @101, @102, @103, @104, @105, @106, @107, @108, @109, @110, @111, @112, @113, @114, @115, @116, @117, @118, @119, @120, @121, @122, @123, @124, @125, @126, @127, @128, @129, @130, @131, @132, @133, @134, @135, @136, @137, @138, @139, @140, @141, @142, @143, @144, @145, @146, @147, @148, @149, @150, @151, @152, @153, @154, @155, @156, @157, @158, @159, @160, @161, @162, @163, @164, @165, @166, @167, @168, @169, @170, @171, @172, @173, @174, @175, @176, @177, @178, @179, @180, @181, @182, @183, @184, @185, @186, @187, @188, @189, @190, @191) */ - rr=w192; + w191 = casadi_sq(w191); + /* #516: @192 = vertcat(@3, @4, @5, @6, @7, @8, @9, @10, @11, @12, @13, @14, + * @15, @16, @17, @18, @19, @20, @21, @22, @23, @24, @25, @26, @27, @28, @29, + * @30, @31, @32, @33, @34, @35, @36, @37, @38, @39, @40, @41, @42, @43, @44, + * @45, @46, @47, @48, @49, @50, @51, @52, @53, @54, @55, @56, @57, @58, @59, + * @60, @61, @62, @63, @64, @65, @66, @67, @68, @69, @70, @71, @72, @73, @74, + * @75, @76, @77, @78, @79, @80, @81, @82, @83, @84, @85, @86, @87, @88, @89, + * @90, @91, @92, @93, @94, @95, @96, @97, @98, @99, @100, @101, @102, @103, + * @104, @105, @106, @107, @108, @109, @110, @111, @112, @113, @114, @115, + * @116, @117, @118, @119, @120, @121, @122, @123, @124, @125, @126, @127, + * @128, @129, @130, @131, @132, @133, @134, @135, @136, @137, @138, @139, + * @140, @141, @142, @143, @144, @145, @146, @147, @148, @149, @150, @151, + * @152, @153, @154, @155, @156, @157, @158, @159, @160, @161, @162, @163, + * @164, @165, @166, @167, @168, @169, @170, @171, @172, @173, @174, @175, + * @176, @177, @178, @179, @180, @181, @182, @183, @184, @185, @186, @187, + * @188, @189, @190, @191) */ + rr = w192; *rr++ = w3; *rr++ = w4; *rr++ = w5; @@ -1364,92 +2082,154 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, *rr++ = w190; *rr++ = w191; /* #517: @0 = mac(@1,@192,@0) */ - for (i=0, rr=w0; i<1; ++i) for (j=0; j<3; ++j, ++rr) for (k=0, ss=w1+j, tt=w192+i*189; k<189; ++k) *rr += ss[k*3]**tt++; + for (i = 0, rr = w0; i < 1; ++i) + for (j = 0; j < 3; ++j, ++rr) + for (k = 0, ss = w1 + j, tt = w192 + i * 189; k < 189; ++k) + *rr += ss[k * 3] * *tt++; /* #518: output[0][0] = @0 */ casadi_copy(w0, 3, res[0]); return 0; } -extern "C" CASADI_SYMBOL_EXPORT int Q_xyz_func(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ +extern "C" CASADI_SYMBOL_EXPORT int Q_xyz_func(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 Q_xyz_func_alloc_mem(void) { - return 0; -} +extern "C" CASADI_SYMBOL_EXPORT int Q_xyz_func_alloc_mem(void) { return 0; } -extern "C" CASADI_SYMBOL_EXPORT int Q_xyz_func_init_mem(int mem) { - return 0; -} +extern "C" CASADI_SYMBOL_EXPORT int Q_xyz_func_init_mem(int mem) { return 0; } -extern "C" CASADI_SYMBOL_EXPORT void Q_xyz_func_free_mem(int mem) { -} +extern "C" CASADI_SYMBOL_EXPORT void Q_xyz_func_free_mem(int mem) {} -extern "C" CASADI_SYMBOL_EXPORT int Q_xyz_func_checkout(void) { - return 0; -} +extern "C" CASADI_SYMBOL_EXPORT int Q_xyz_func_checkout(void) { return 0; } -extern "C" CASADI_SYMBOL_EXPORT void Q_xyz_func_release(int mem) { -} +extern "C" CASADI_SYMBOL_EXPORT void Q_xyz_func_release(int mem) {} -extern "C" CASADI_SYMBOL_EXPORT void Q_xyz_func_incref(void) { -} +extern "C" CASADI_SYMBOL_EXPORT void Q_xyz_func_incref(void) {} -extern "C" CASADI_SYMBOL_EXPORT void Q_xyz_func_decref(void) { -} +extern "C" CASADI_SYMBOL_EXPORT void Q_xyz_func_decref(void) {} -extern "C" CASADI_SYMBOL_EXPORT casadi_int Q_xyz_func_n_in(void) { return 1;} +extern "C" CASADI_SYMBOL_EXPORT casadi_int Q_xyz_func_n_in(void) { return 1; } -extern "C" CASADI_SYMBOL_EXPORT casadi_int Q_xyz_func_n_out(void) { return 1;} +extern "C" CASADI_SYMBOL_EXPORT casadi_int Q_xyz_func_n_out(void) { return 1; } -extern "C" CASADI_SYMBOL_EXPORT casadi_real Q_xyz_func_default_in(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT casadi_real +Q_xyz_func_default_in(casadi_int i) { switch (i) { - default: return 0; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT const char* Q_xyz_func_name_in(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT const char *Q_xyz_func_name_in(casadi_int i) { switch (i) { - case 0: return "i0"; - default: return 0; + case 0: + return "i0"; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT const char* Q_xyz_func_name_out(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT const char *Q_xyz_func_name_out(casadi_int i) { switch (i) { - case 0: return "o0"; - default: return 0; + case 0: + return "o0"; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT const casadi_int* Q_xyz_func_sparsity_in(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT const casadi_int * +Q_xyz_func_sparsity_in(casadi_int i) { switch (i) { - case 0: return casadi_s0; - default: return 0; + case 0: + return casadi_s0; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT const casadi_int* Q_xyz_func_sparsity_out(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT const casadi_int * +Q_xyz_func_sparsity_out(casadi_int i) { switch (i) { - case 0: return casadi_s1; - default: return 0; + case 0: + return casadi_s1; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT int Q_xyz_func_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 190; - if (sz_res) *sz_res = 2; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 969; +extern "C" CASADI_SYMBOL_EXPORT int Q_xyz_func_work(casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w) { + if (sz_arg) + *sz_arg = 190; + if (sz_res) + *sz_res = 2; + if (sz_iw) + *sz_iw = 0; + if (sz_w) + *sz_w = 969; return 0; } /* jac_Q_xyz_func:(i0[18],out_o0[3x1,0nz])->(jac_o0_i0[3x18]) */ -static int casadi_f1(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { +static int casadi_f1(const casadi_real **arg, casadi_real **res, casadi_int *iw, + casadi_real *w, int mem) { casadi_int i, j; casadi_real *rr, *ss; const casadi_real *cs; - casadi_real *w0=w+189, *w1=w+243, *w2=w+261, w3, *w4=w+280, *w5=w+469, *w6=w+1036, w7, w8, w9, w10, w11, w12, w13, w14, w15, w16, w17, w18, w19, w20, w21, w22, w23, w24, w25, w26, w27, w28, w29, w30, w31, w32, w33, w34, w35, w36, w37, w38, w39, w40, w41, w42, w43, w44, w45, w46, w47, w48, w49, w50, w51, w52, w53, w54, w55, w56, w57, w58, w59, w60, w61, w62, w63, w64, w65, w66, w67, w68, w69, w70, w71, w72, w73, w74, w75, w76, w77, w78, w79, w80, w81, w82, w83, w84, w85, w86, w87, w88, w89, w90, w91, w92, w93, w94, w95, w96, w97, w98, w99, w100, w101, w102, w103, w104, w105, w106, w107, w108, w109, w110, w111, w112, w113, w114, w115, w116, w117, w118, w119, w120, w121, w122, w123, w124, w125, w126, w127, w128, w129, w130, w131, w132, w133, w134, w135, w136, w137, w138, w139, w140, w141, w142, w143, w144, w145, w146, w147, w148, w149, w150, w151, w152, w153, w154, w155, w156, w157, w158, w159, w160, w161, w162, w163, w164, w165, w166, w167, w168, w169, w170, w171, w172, w173, w174, w175, w176, w177, w178, w179, w180, w181, w182, w183, w184, w185, w186, w187, w188, w189, w190, w191, w192, w193, w194, w195, w196, w197, w198, w199, w200, w201, w202, w203, w204, w205, w206, w207, w208, w209, w210, w211, w212, w213, w214, w215, w216, w217, w218, w219, w220, w221, w222, w223, w224, w225, w226, w227, w228, w229, w230, w231, w232, w233, w234, w235, w236, w237, w238, w239, w240, w241, w242, w243, w244, w245, w246, w247, w248, w249, w250, w251, w252, w253, w254, w255, w256, w257, w258, w259, w260, w261, w262, w263, w264, w265, w266, w267, w268, w269, w270, w271, w272, w273, w274, w275, w276, w277, w278, w279, w280, w281, w282, w283, w284, w285, w286, w287, w288, w289, w290, w291, w292, w293, w294, w295, w296, w297, w298, w299, w300, w301, w302, w303, w304, w305, w306, w307, w308, w309, w310, w311, w312, w313, w314, w315, w316, w317, w318, w319, w320, w321, w322, w323, w324, w325, w326, w327, w328, w329, w330, w331, w332, w333, w334, w335, w336, w337, w338, w339, w340, w341, w342, w343, w344, w345, w346, w347, w348, w349, w350, w351, w352, w353, w354, w355, w356, w357, w358, w359, w360, w361, w362, w363, w364, w365, w366, w367, w368, w369, w370, w371, w372, w373, w374, w375, w376, w377, w378, w379, w380, w381, w382, w383, w384, w385, w386, w387, w388, w389, w390, w391, w392, w393, w394, w395, w396, w397, w398, w399, w400, w401, w402, w403, w404, w405, w406, w407, w408, w409, w410, w411, w412, w413, w414, w415, w416, w417, w418, w419, w420, w421, w422, w423, w424, w425, w426, w427, w428, w429, w430, w431, w432, w433, w434, w435, w436, w437, w438, w439, w440, w441, w442, w443, w444, w445, w446, w447, w448, w449, w450, w451, w452, w453, w454, w455, w456, w457, w458, w459, w460, w461, w462, w463, w464, w465, w466, w467, w468, w469, w470, w471, w472, w473, w474, w475, w476, w477, w478, w479, w480, w481, w482, w483, w484, w485, w486, w487, w488, w489, w490, w491, w492, w493, w494, w495, w496, w497, w498, w499, w500, w501, w502, w503, w504, w505, w506, w507, w508, w509, w510, w511, w512, w513, w514, w515, w516, w517, w518, *w519=w+2115; + casadi_real *w0 = w + 189, *w1 = w + 243, *w2 = w + 261, w3, *w4 = w + 280, + *w5 = w + 469, *w6 = w + 1036, w7, w8, w9, w10, w11, w12, w13, + w14, w15, w16, w17, w18, w19, w20, w21, w22, w23, w24, w25, w26, + w27, w28, w29, w30, w31, w32, w33, w34, w35, w36, w37, w38, w39, + w40, w41, w42, w43, w44, w45, w46, w47, w48, w49, w50, w51, w52, + w53, w54, w55, w56, w57, w58, w59, w60, w61, w62, w63, w64, w65, + w66, w67, w68, w69, w70, w71, w72, w73, w74, w75, w76, w77, w78, + w79, w80, w81, w82, w83, w84, w85, w86, w87, w88, w89, w90, w91, + w92, w93, w94, w95, w96, w97, w98, w99, w100, w101, w102, w103, + w104, w105, w106, w107, w108, w109, w110, w111, w112, w113, w114, + w115, w116, w117, w118, w119, w120, w121, w122, w123, w124, w125, + w126, w127, w128, w129, w130, w131, w132, w133, w134, w135, w136, + w137, w138, w139, w140, w141, w142, w143, w144, w145, w146, w147, + w148, w149, w150, w151, w152, w153, w154, w155, w156, w157, w158, + w159, w160, w161, w162, w163, w164, w165, w166, w167, w168, w169, + w170, w171, w172, w173, w174, w175, w176, w177, w178, w179, w180, + w181, w182, w183, w184, w185, w186, w187, w188, w189, w190, w191, + w192, w193, w194, w195, w196, w197, w198, w199, w200, w201, w202, + w203, w204, w205, w206, w207, w208, w209, w210, w211, w212, w213, + w214, w215, w216, w217, w218, w219, w220, w221, w222, w223, w224, + w225, w226, w227, w228, w229, w230, w231, w232, w233, w234, w235, + w236, w237, w238, w239, w240, w241, w242, w243, w244, w245, w246, + w247, w248, w249, w250, w251, w252, w253, w254, w255, w256, w257, + w258, w259, w260, w261, w262, w263, w264, w265, w266, w267, w268, + w269, w270, w271, w272, w273, w274, w275, w276, w277, w278, w279, + w280, w281, w282, w283, w284, w285, w286, w287, w288, w289, w290, + w291, w292, w293, w294, w295, w296, w297, w298, w299, w300, w301, + w302, w303, w304, w305, w306, w307, w308, w309, w310, w311, w312, + w313, w314, w315, w316, w317, w318, w319, w320, w321, w322, w323, + w324, w325, w326, w327, w328, w329, w330, w331, w332, w333, w334, + w335, w336, w337, w338, w339, w340, w341, w342, w343, w344, w345, + w346, w347, w348, w349, w350, w351, w352, w353, w354, w355, w356, + w357, w358, w359, w360, w361, w362, w363, w364, w365, w366, w367, + w368, w369, w370, w371, w372, w373, w374, w375, w376, w377, w378, + w379, w380, w381, w382, w383, w384, w385, w386, w387, w388, w389, + w390, w391, w392, w393, w394, w395, w396, w397, w398, w399, w400, + w401, w402, w403, w404, w405, w406, w407, w408, w409, w410, w411, + w412, w413, w414, w415, w416, w417, w418, w419, w420, w421, w422, + w423, w424, w425, w426, w427, w428, w429, w430, w431, w432, w433, + w434, w435, w436, w437, w438, w439, w440, w441, w442, w443, w444, + w445, w446, w447, w448, w449, w450, w451, w452, w453, w454, w455, + w456, w457, w458, w459, w460, w461, w462, w463, w464, w465, w466, + w467, w468, w469, w470, w471, w472, w473, w474, w475, w476, w477, + w478, w479, w480, w481, w482, w483, w484, w485, w486, w487, w488, + w489, w490, w491, w492, w493, w494, w495, w496, w497, w498, w499, + w500, w501, w502, w503, w504, w505, w506, w507, w508, w509, w510, + w511, w512, w513, w514, w515, w516, w517, w518, *w519 = w + 2115; /* #0: @0 = zeros(18x3) */ casadi_clear(w0, 54); /* #1: @1 = zeros(18x1) */ @@ -1457,23 +2237,131 @@ static int casadi_f1(const casadi_real** arg, casadi_real** res, casadi_int* iw, /* #2: @2 = input[0][0] */ casadi_copy(arg[0], 18, w2); /* #3: @3 = @2[17] */ - for (rr=(&w3), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w3), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #4: @3 = (2.*@3) */ - w3 = (2.* w3 ); + w3 = (2. * w3); /* #5: @4 = zeros(189x1) */ casadi_clear(w4, 189); - /* #6: @5 = - [[-0.0627168, 0.00911909, 0.113161, 0.0429596, 0.0215499, -0.0190236, 0.00690652, -0.00189939, 0.002902, 0.0607266, -0.00212102, 0.12994, 0.0448556, 0.0249316, -0.00230252, -0.00362391, 0.0010149, 0.00561049, 0.00738982, 0.0688731, 0.00180948, 0.00582719, 0.00732466, -0.000265723, 0.00317307, -0.000477502, 0.000614032, -0.000308676, 0.00728648, 0.00378877, -0.000788835, -0.00171585, -0.000390278, 0.00067807, 8.72168e-05, -0.000138241, 0.000108059, 0.0157792, -0.000739303, 0.00578389, 0.00424382, 0.00213685, 0.00126477, 0.00206954, 0.00615703, 0.00169051, -0.0097154, -0.000728647, -0.00357184, -0.00681516, 0.00484937, 0.00155446, -2.22236e-05, -8.87748e-05, -0.0132386, -0.0103304, -0.00186689, -0.0010376, -0.000515239, -0.00050907, -0.00123645, 0.00840625, 0.00571376, 0.00199724, 0.00370229, 0.0084076, 0.00237273, 0.000642649, 0.00210247, -0.00912764, -0.00753473, -0.000724963, -0.00100813, -0.000475941, -2.6334e-05, 0.00132596, 0.000911836, 0.00361837, 0.00198928, 0.00132822, 0.00239418, 0.00122732, 0.000254297, 0.000857163, -0.00594194, 3.62108e-05, -0.00021935, -0.000182416, 0.000107603, 0.00269593, 0.00375798, 0.00114746, 0.000627986, -3.32492e-05, 0.000411532, 0.000765713, 0.000151893, 0.000185052, -0.00903996, 0.0135454, -0.000722359, -0.00490391, 0.00203116, 0.00710772, 0.0076969, 0.00173302, 0.00100631, 0.00420003, 0.000574205, -0.000373772, 0.00102787, -0.00561679, -0.0023325, -0.00431477, 0.00044308, 0.00322925, -0.00353879, -0.00194758, -0.000349419, -0.00114885, 0.00218443, 0.000315052, -0.000246945, -0.000234972, 0.00343182, -0.000576893, 0.00128453, -0.000627002, -0.000244513, -7.21641e-05, 0.000186332, 0.000150758, -0.000141313, 7.20824e-05, -0.00178129, 0.000312008, 0.00118627, 0.00151454, 0.000523475, 0.000436795, 0.00116518, 0.000607068, 3.05045e-05, 0.000184028, 0.0117549, 0.0796893, -0.00214423, -0.00525946, -0.00403565, 0.000691185, 0.00321344, -8.39683e-05, 0.000361989, -0.00329383, -0.010862, -0.000101139, -0.00339473, -0.00643871, 0.00571431, 0.00230936, -0.00149976, -0.00183948, -0.0151343, -0.0121317, -0.00300954, 0.00254861, 0.000666956, -0.00133131, -0.0101003, -0.00841691, -0.000414823, 0.000461035, 0.00029282, -0.000105389, -0.00673323, -0.000421961, 0.00101527, 0.000355623, -9.10409e-05, -0.00796872, -0.00509959, 0.000959568, -0.00373789, -0.00553031, -0.00328613, 0.00408343, 0.000243895, -0.00241938, -0.00116566], - [0.0186502, -0.0152338, 0.113699, 0.0327646, 0.020499, 0.108095, 0.00313986, 0.00353094, 0.0219446, 0.0203845, -0.0201273, -0.107101, -0.027541, -0.0184919, -0.11181, 0.00255492, 0.00226364, -0.0213281, 0.00436631, 0.0782532, -0.0103053, -0.00250494, -0.00615842, 0.000778518, -0.00575801, -0.000519435, -0.00156735, 0.00318815, -0.000309406, 0.00563101, 0.000204443, 5.68998e-05, -0.000929549, -0.00403978, -0.00091885, 0.000161048, -0.00316028, -0.0334966, 0.00850775, -0.0130498, -0.000829289, 0.00390585, 0.00226736, -0.00122417, 0.00455176, 0.00103396, 0.0175465, 0.000130719, 0.00269078, 0.00230771, -0.00480044, -0.000810291, -0.000815402, 0.00352587, -0.0104141, -0.00833722, -0.005023, 0.00403064, 0.00142261, -0.00163339, -0.00459653, 0.0153418, -0.00608615, -0.00163041, 0.00151932, -0.00262283, 0.0138738, 0.00181605, -0.00365686, -0.00782621, -0.00683235, -0.000510232, 0.0017275, 0.000173605, 0.000342975, -0.00261291, -0.00100338, -0.00121781, -0.00026139, -4.05719e-05, -0.00145568, 0.0055267, 0.000959155, -0.00107796, -0.00583856, 0.00117568, 0.00183215, 0.000408175, 0.00088, -0.000102079, 0.00278011, -0.00347983, -0.000377072, -0.000156108, -0.00110755, 0.00160366, 7.10275e-05, -0.000213424, 0.0112871, -0.0382876, 1.51803e-05, 0.0107112, -0.00273886, 0.00238821, 0.000870183, 0.000674078, 0.000674308, 9.9041e-05, 0.0010109, 0.000694078, -0.000421943, 0.00352808, -0.00459972, 0.0054003, 0.0056865, 0.00739394, 0.014655, 0.00521551, 0.00175918, 0.00134017, -4.93111e-05, 0.000393203, -0.000179556, 0.00324963, -0.00830727, 0.00117472, 0.00104853, 0.00224924, 0.00096319, 0.000193481, 0.000932703, -0.00048353, -1.45417e-05, -0.000183356, 0.00493695, -0.00059341, -0.00117813, 0.00318192, 0.000904924, 9.0911e-06, 0.000364803, -0.00028243, -0.000141499, -4.08834e-05, -0.00241024, -0.062841, -0.0194669, -0.00606772, -0.00765537, 0.000140633, 0.0063877, -0.000249763, -0.00191831, 0.00376216, -0.034627, 0.00946453, -0.0129204, -0.000289844, -0.00292233, -0.00266214, -0.0014625, -0.0102087, 0.0042977, 0.00396114, 0.0021332, 0.00494168, 0.00123231, 0.000931988, 0.00571952, 0.00509258, -0.00024296, 0.00221941, 0.000214861, -0.000477062, 0.00446551, -0.0012799, 0.00167925, 0.000267742, -0.000903358, -0.0116068, -0.0399685, -0.000686781, -0.0111621, -0.00392442, 0.00435219, 0.00613091, -0.00343162, -0.00873929, -0.00498964], - [0.00698102, 0.17203, -0.135309, -0.0530585, -0.0274505, -0.0986184, 0.0199336, -0.0157709, -0.0144523, -0.0057738, 0.225099, 0.0442009, 0.0109938, 0.0135212, 0.130972, 0.00595479, -0.0246616, 0.0116247, 0.0376314, 0.00219893, -0.17391, -0.0377317, 0.00496186, 0.00381288, 0.0101307, -0.00426805, 0.00579589, -0.0141228, 0.0269361, -0.00380837, 0.000902046, -0.00042646, -0.00801859, 0.020572, 0.0039739, -0.00696142, 0.0276635, 0.0476737, -0.00153598, 0.00592851, -0.0216158, -0.014092, -0.00735922, 0.00370974, -0.0538922, 0.00122146, -0.0433639, -0.00608319, -0.00581932, -0.0254123, 0.0199356, 0.0106324, 0.00251351, 0.0192596, 0.0220464, 0.0151556, -0.0099076, -0.00955052, -0.00566445, 0.00436284, 0.0244606, -0.00943798, 0.013099, -0.00128335, 9.90331e-05, -0.0107958, -0.00261728, 0.00379955, -0.00506443, 0.0135961, 0.0105554, -0.00377641, -0.00402787, -0.000316862, 0.00159515, 0.00603265, 0.00461063, 0.00259039, -0.00104727, -0.00086985, -0.00191415, -0.000168667, 0.00097723, -0.00154505, 0.00751481, -0.00186548, 0.00124077, 0.000453952, -0.000606508, 0.00533578, 0.00218407, 0.00453982, 0.00082932, 0.00100676, 0.00185951, -0.0013692, 0.000262502, -0.00179054, 0.00876282, 0.0492979, 0.0125155, -0.00136387, -0.000319577, -0.011553, 0.0106951, 0.00197776, -0.000175277, -0.000175856, -0.00428024, -0.00248809, 0.000817511, 0.0118462, 0.00909968, -0.001888, -0.0199842, -0.0197348, 0.00597335, 0.00265272, -0.000482235, -0.00247393, -0.0019193, 0.00232815, 0.00360518, -0.00251869, -0.00537591, -0.00307388, -0.00700011, 0.00522888, 0.000153726, 0.00022249, -0.00224076, -0.00474255, -0.000131328, -0.000530219, 0.000999337, -0.00765933, 0.00748947, 0.00385722, 0.00053268, 0.00158917, -0.000789119, 0.00316381, -0.000937859, 4.05151e-05, -0.0294117, -0.0818586, -0.153856, -0.0288832, 0.00859953, 0.0104256, -0.0222873, 0.000147819, 0.00422314, -0.00698901, 0.0560896, -0.00814602, -0.00126066, -0.0382772, 0.00978097, 0.00476111, -0.00192896, 0.00138786, 0.00727908, 0.00629046, 0.0205368, -0.0193913, -0.00771905, -0.00555957, -0.000546143, -0.000160415, 0.00851434, -0.00715185, -0.00163088, -0.000876829, 0.000225337, 0.00403854, -0.00366158, -0.000243572, 0.000672828, 0.00148787, 0.0589061, 0.00907169, 0.00627746, -0.00714632, -0.00638265, -0.00791136, 0.0035721, -0.00164952, 0.00134629]] */ + /* #6: @5 = + [[-0.0627168, 0.00911909, 0.113161, 0.0429596, 0.0215499, -0.0190236, + 0.00690652, -0.00189939, 0.002902, 0.0607266, -0.00212102, 0.12994, 0.0448556, + 0.0249316, -0.00230252, -0.00362391, 0.0010149, 0.00561049, 0.00738982, + 0.0688731, 0.00180948, 0.00582719, 0.00732466, -0.000265723, 0.00317307, + -0.000477502, 0.000614032, -0.000308676, 0.00728648, 0.00378877, -0.000788835, + -0.00171585, -0.000390278, 0.00067807, 8.72168e-05, -0.000138241, 0.000108059, + 0.0157792, -0.000739303, 0.00578389, 0.00424382, 0.00213685, 0.00126477, + 0.00206954, 0.00615703, 0.00169051, -0.0097154, -0.000728647, -0.00357184, + -0.00681516, 0.00484937, 0.00155446, -2.22236e-05, -8.87748e-05, -0.0132386, + -0.0103304, -0.00186689, -0.0010376, -0.000515239, -0.00050907, -0.00123645, + 0.00840625, 0.00571376, 0.00199724, 0.00370229, 0.0084076, 0.00237273, + 0.000642649, 0.00210247, -0.00912764, -0.00753473, -0.000724963, -0.00100813, + -0.000475941, -2.6334e-05, 0.00132596, 0.000911836, 0.00361837, 0.00198928, + 0.00132822, 0.00239418, 0.00122732, 0.000254297, 0.000857163, + -0.00594194, 3.62108e-05, -0.00021935, -0.000182416, 0.000107603, 0.00269593, + 0.00375798, 0.00114746, 0.000627986, -3.32492e-05, 0.000411532, 0.000765713, + 0.000151893, 0.000185052, -0.00903996, 0.0135454, -0.000722359, -0.00490391, + 0.00203116, 0.00710772, 0.0076969, 0.00173302, 0.00100631, 0.00420003, + 0.000574205, -0.000373772, 0.00102787, -0.00561679, -0.0023325, -0.00431477, + 0.00044308, 0.00322925, -0.00353879, -0.00194758, -0.000349419, -0.00114885, + 0.00218443, 0.000315052, -0.000246945, -0.000234972, 0.00343182, -0.000576893, + 0.00128453, -0.000627002, -0.000244513, -7.21641e-05, 0.000186332, + 0.000150758, -0.000141313, 7.20824e-05, -0.00178129, 0.000312008, 0.00118627, + 0.00151454, 0.000523475, 0.000436795, 0.00116518, 0.000607068, 3.05045e-05, + 0.000184028, 0.0117549, 0.0796893, -0.00214423, -0.00525946, -0.00403565, + 0.000691185, 0.00321344, -8.39683e-05, 0.000361989, -0.00329383, -0.010862, + -0.000101139, -0.00339473, -0.00643871, 0.00571431, 0.00230936, -0.00149976, + -0.00183948, -0.0151343, -0.0121317, -0.00300954, 0.00254861, 0.000666956, + -0.00133131, -0.0101003, -0.00841691, -0.000414823, 0.000461035, 0.00029282, + -0.000105389, -0.00673323, -0.000421961, 0.00101527, 0.000355623, + -9.10409e-05, -0.00796872, -0.00509959, 0.000959568, -0.00373789, -0.00553031, + -0.00328613, 0.00408343, 0.000243895, -0.00241938, -0.00116566], [0.0186502, + -0.0152338, 0.113699, 0.0327646, 0.020499, 0.108095, 0.00313986, 0.00353094, + 0.0219446, 0.0203845, -0.0201273, -0.107101, -0.027541, -0.0184919, -0.11181, + 0.00255492, 0.00226364, -0.0213281, 0.00436631, 0.0782532, -0.0103053, + -0.00250494, -0.00615842, 0.000778518, -0.00575801, -0.000519435, -0.00156735, + 0.00318815, -0.000309406, 0.00563101, 0.000204443, 5.68998e-05, -0.000929549, + -0.00403978, -0.00091885, 0.000161048, -0.00316028, -0.0334966, 0.00850775, + -0.0130498, -0.000829289, 0.00390585, 0.00226736, -0.00122417, 0.00455176, + 0.00103396, 0.0175465, 0.000130719, 0.00269078, 0.00230771, -0.00480044, + -0.000810291, -0.000815402, 0.00352587, -0.0104141, -0.00833722, -0.005023, + 0.00403064, 0.00142261, -0.00163339, -0.00459653, 0.0153418, -0.00608615, + -0.00163041, 0.00151932, -0.00262283, 0.0138738, 0.00181605, -0.00365686, + -0.00782621, -0.00683235, -0.000510232, 0.0017275, 0.000173605, 0.000342975, + -0.00261291, -0.00100338, -0.00121781, -0.00026139, -4.05719e-05, -0.00145568, + 0.0055267, 0.000959155, -0.00107796, -0.00583856, 0.00117568, 0.00183215, + 0.000408175, 0.00088, -0.000102079, 0.00278011, -0.00347983, -0.000377072, + -0.000156108, -0.00110755, 0.00160366, 7.10275e-05, -0.000213424, 0.0112871, + -0.0382876, 1.51803e-05, 0.0107112, -0.00273886, 0.00238821, 0.000870183, + 0.000674078, 0.000674308, 9.9041e-05, 0.0010109, 0.000694078, -0.000421943, + 0.00352808, -0.00459972, 0.0054003, 0.0056865, 0.00739394, 0.014655, + 0.00521551, 0.00175918, 0.00134017, -4.93111e-05, 0.000393203, -0.000179556, + 0.00324963, -0.00830727, 0.00117472, 0.00104853, 0.00224924, 0.00096319, + 0.000193481, 0.000932703, -0.00048353, -1.45417e-05, -0.000183356, 0.00493695, + -0.00059341, -0.00117813, 0.00318192, 0.000904924, 9.0911e-06, 0.000364803, + -0.00028243, -0.000141499, -4.08834e-05, -0.00241024, -0.062841, -0.0194669, + -0.00606772, -0.00765537, 0.000140633, 0.0063877, -0.000249763, -0.00191831, + 0.00376216, -0.034627, 0.00946453, -0.0129204, -0.000289844, -0.00292233, + -0.00266214, -0.0014625, -0.0102087, 0.0042977, 0.00396114, 0.0021332, + 0.00494168, 0.00123231, 0.000931988, 0.00571952, 0.00509258, -0.00024296, + 0.00221941, 0.000214861, -0.000477062, 0.00446551, -0.0012799, 0.00167925, + 0.000267742, -0.000903358, -0.0116068, -0.0399685, -0.000686781, -0.0111621, + -0.00392442, 0.00435219, 0.00613091, -0.00343162, -0.00873929, -0.00498964], + [0.00698102, 0.17203, -0.135309, -0.0530585, -0.0274505, -0.0986184, + 0.0199336, -0.0157709, -0.0144523, -0.0057738, 0.225099, 0.0442009, 0.0109938, + 0.0135212, 0.130972, 0.00595479, -0.0246616, 0.0116247, 0.0376314, 0.00219893, + -0.17391, -0.0377317, 0.00496186, 0.00381288, 0.0101307, -0.00426805, + 0.00579589, -0.0141228, 0.0269361, -0.00380837, 0.000902046, -0.00042646, + -0.00801859, 0.020572, 0.0039739, -0.00696142, 0.0276635, 0.0476737, + -0.00153598, 0.00592851, -0.0216158, -0.014092, -0.00735922, 0.00370974, + -0.0538922, 0.00122146, -0.0433639, -0.00608319, -0.00581932, -0.0254123, + 0.0199356, 0.0106324, 0.00251351, 0.0192596, 0.0220464, 0.0151556, -0.0099076, + -0.00955052, -0.00566445, 0.00436284, 0.0244606, -0.00943798, 0.013099, + -0.00128335, 9.90331e-05, -0.0107958, -0.00261728, 0.00379955, -0.00506443, + 0.0135961, 0.0105554, -0.00377641, -0.00402787, -0.000316862, 0.00159515, + 0.00603265, 0.00461063, 0.00259039, -0.00104727, -0.00086985, -0.00191415, + -0.000168667, 0.00097723, -0.00154505, 0.00751481, -0.00186548, 0.00124077, + 0.000453952, -0.000606508, 0.00533578, 0.00218407, 0.00453982, 0.00082932, + 0.00100676, 0.00185951, -0.0013692, 0.000262502, -0.00179054, 0.00876282, + 0.0492979, 0.0125155, -0.00136387, -0.000319577, -0.011553, 0.0106951, + 0.00197776, -0.000175277, -0.000175856, -0.00428024, -0.00248809, 0.000817511, + 0.0118462, 0.00909968, -0.001888, -0.0199842, -0.0197348, 0.00597335, + 0.00265272, -0.000482235, -0.00247393, -0.0019193, 0.00232815, 0.00360518, + -0.00251869, -0.00537591, -0.00307388, -0.00700011, 0.00522888, 0.000153726, + 0.00022249, -0.00224076, -0.00474255, -0.000131328, -0.000530219, 0.000999337, + -0.00765933, 0.00748947, 0.00385722, 0.00053268, 0.00158917, -0.000789119, + 0.00316381, -0.000937859, 4.05151e-05, -0.0294117, -0.0818586, -0.153856, + -0.0288832, 0.00859953, 0.0104256, -0.0222873, 0.000147819, 0.00422314, + -0.00698901, 0.0560896, -0.00814602, -0.00126066, -0.0382772, 0.00978097, + 0.00476111, -0.00192896, 0.00138786, 0.00727908, 0.00629046, 0.0205368, + -0.0193913, -0.00771905, -0.00555957, -0.000546143, -0.000160415, 0.00851434, + -0.00715185, -0.00163088, -0.000876829, 0.000225337, 0.00403854, -0.00366158, + -0.000243572, 0.000672828, 0.00148787, 0.0589061, 0.00907169, 0.00627746, + -0.00714632, -0.00638265, -0.00791136, 0.0035721, -0.00164952, 0.00134629]] */ casadi_copy(casadi_c0, 567, w5); /* #7: @6 = @5' */ - for (i=0, rr=w6, cs=w5; i<189; ++i) for (j=0; j<3; ++j) rr[i+j*189] = *cs++; + for (i = 0, rr = w6, cs = w5; i < 189; ++i) + for (j = 0; j < 3; ++j) + rr[i + j * 189] = *cs++; /* #8: @7 = ones(3x1,1nz) */ w7 = 1.; /* #9: @4 = mac(@6,@7,@4) */ casadi_mtimes(w6, casadi_s2, (&w7), casadi_s3, w4, casadi_s4, w, 0); - /* #10: {@7, @8, @9, @10, @11, @12, @13, @14, @15, @16, @17, @18, @19, @20, @21, @22, @23, @24, @25, @26, @27, @28, @29, @30, @31, @32, @33, @34, @35, @36, @37, @38, @39, @40, @41, @42, @43, @44, @45, @46, @47, @48, @49, @50, @51, @52, @53, @54, @55, @56, @57, @58, @59, @60, @61, @62, @63, @64, @65, @66, @67, @68, @69, @70, @71, @72, @73, @74, @75, @76, @77, @78, @79, @80, @81, @82, @83, @84, @85, @86, @87, @88, @89, @90, @91, @92, @93, @94, @95, @96, @97, @98, @99, @100, @101, @102, @103, @104, @105, @106, @107, @108, @109, @110, @111, @112, @113, @114, @115, @116, @117, @118, @119, @120, @121, @122, @123, @124, @125, @126, @127, @128, @129, @130, @131, @132, @133, @134, @135, @136, @137, @138, @139, @140, @141, @142, @143, @144, @145, @146, @147, @148, @149, @150, @151, @152, @153, @154, @155, @156, @157, @158, @159, @160, @161, @162, @163, @164, @165, @166, @167, @168, @169, @170, @171, @172, @173, @174, @175, @176, @177, @178, @179, @180, @181, @182, @183, @184, @185, @186, @187, @188, @189, @190, @191, @192, @193, @194, @195} = vertsplit(@4) */ + /* #10: {@7, @8, @9, @10, @11, @12, @13, @14, @15, @16, @17, @18, @19, @20, + * @21, @22, @23, @24, @25, @26, @27, @28, @29, @30, @31, @32, @33, @34, @35, + * @36, @37, @38, @39, @40, @41, @42, @43, @44, @45, @46, @47, @48, @49, @50, + * @51, @52, @53, @54, @55, @56, @57, @58, @59, @60, @61, @62, @63, @64, @65, + * @66, @67, @68, @69, @70, @71, @72, @73, @74, @75, @76, @77, @78, @79, @80, + * @81, @82, @83, @84, @85, @86, @87, @88, @89, @90, @91, @92, @93, @94, @95, + * @96, @97, @98, @99, @100, @101, @102, @103, @104, @105, @106, @107, @108, + * @109, @110, @111, @112, @113, @114, @115, @116, @117, @118, @119, @120, + * @121, @122, @123, @124, @125, @126, @127, @128, @129, @130, @131, @132, + * @133, @134, @135, @136, @137, @138, @139, @140, @141, @142, @143, @144, + * @145, @146, @147, @148, @149, @150, @151, @152, @153, @154, @155, @156, + * @157, @158, @159, @160, @161, @162, @163, @164, @165, @166, @167, @168, + * @169, @170, @171, @172, @173, @174, @175, @176, @177, @178, @179, @180, + * @181, @182, @183, @184, @185, @186, @187, @188, @189, @190, @191, @192, + * @193, @194, @195} = vertsplit(@4) */ w7 = w4[0]; w8 = w4[1]; w9 = w4[2]; @@ -1664,2100 +2552,2818 @@ static int casadi_f1(const casadi_real** arg, casadi_real** res, casadi_int* iw, w194 = w4[187]; w195 = w4[188]; /* #11: @195 = (@3*@195) */ - w195 = (w3*w195); + w195 = (w3 * w195); /* #12: (@1[17] += @195) */ - for (rr=w1+17, ss=(&w195); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w195); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #13: @195 = @2[16] */ - for (rr=(&w195), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w195), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #14: @196 = (@195*@194) */ - w196 = (w195*w194); + w196 = (w195 * w194); /* #15: (@1[17] += @196) */ - for (rr=w1+17, ss=(&w196); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w196); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #16: @196 = @2[17] */ - for (rr=(&w196), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w196), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #17: @194 = (@196*@194) */ - w194 = (w196*w194); + w194 = (w196 * w194); /* #18: (@1[16] += @194) */ - for (rr=w1+16, ss=(&w194); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w194); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #19: @194 = @2[16] */ - for (rr=(&w194), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w194), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #20: @194 = (2.*@194) */ - w194 = (2.* w194 ); + w194 = (2. * w194); /* #21: @193 = (@194*@193) */ - w193 = (w194*w193); + w193 = (w194 * w193); /* #22: (@1[16] += @193) */ - for (rr=w1+16, ss=(&w193); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w193); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #23: @193 = @2[15] */ - for (rr=(&w193), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w193), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #24: @197 = (@193*@192) */ - w197 = (w193*w192); + w197 = (w193 * w192); /* #25: (@1[17] += @197) */ - for (rr=w1+17, ss=(&w197); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w197); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #26: @197 = @2[17] */ - for (rr=(&w197), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w197), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #27: @192 = (@197*@192) */ - w192 = (w197*w192); + w192 = (w197 * w192); /* #28: (@1[15] += @192) */ - for (rr=w1+15, ss=(&w192); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w192); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #29: @192 = @2[15] */ - for (rr=(&w192), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w192), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #30: @198 = (@192*@191) */ - w198 = (w192*w191); + w198 = (w192 * w191); /* #31: (@1[16] += @198) */ - for (rr=w1+16, ss=(&w198); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w198); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #32: @198 = @2[16] */ - for (rr=(&w198), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w198), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #33: @191 = (@198*@191) */ - w191 = (w198*w191); + w191 = (w198 * w191); /* #34: (@1[15] += @191) */ - for (rr=w1+15, ss=(&w191); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w191); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #35: @191 = @2[15] */ - for (rr=(&w191), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w191), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #36: @191 = (2.*@191) */ - w191 = (2.* w191 ); + w191 = (2. * w191); /* #37: @190 = (@191*@190) */ - w190 = (w191*w190); + w190 = (w191 * w190); /* #38: (@1[15] += @190) */ - for (rr=w1+15, ss=(&w190); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w190); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #39: @190 = @2[14] */ - for (rr=(&w190), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w190), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #40: @199 = (@190*@189) */ - w199 = (w190*w189); + w199 = (w190 * w189); /* #41: (@1[17] += @199) */ - for (rr=w1+17, ss=(&w199); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w199); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #42: @199 = @2[17] */ - for (rr=(&w199), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w199), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #43: @189 = (@199*@189) */ - w189 = (w199*w189); + w189 = (w199 * w189); /* #44: (@1[14] += @189) */ - for (rr=w1+14, ss=(&w189); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w189); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #45: @189 = @2[14] */ - for (rr=(&w189), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w189), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #46: @200 = (@189*@188) */ - w200 = (w189*w188); + w200 = (w189 * w188); /* #47: (@1[16] += @200) */ - for (rr=w1+16, ss=(&w200); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w200); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #48: @200 = @2[16] */ - for (rr=(&w200), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w200), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #49: @188 = (@200*@188) */ - w188 = (w200*w188); + w188 = (w200 * w188); /* #50: (@1[14] += @188) */ - for (rr=w1+14, ss=(&w188); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w188); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #51: @188 = @2[14] */ - for (rr=(&w188), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w188), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #52: @201 = (@188*@187) */ - w201 = (w188*w187); + w201 = (w188 * w187); /* #53: (@1[15] += @201) */ - for (rr=w1+15, ss=(&w201); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w201); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #54: @201 = @2[15] */ - for (rr=(&w201), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w201), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #55: @187 = (@201*@187) */ - w187 = (w201*w187); + w187 = (w201 * w187); /* #56: (@1[14] += @187) */ - for (rr=w1+14, ss=(&w187); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w187); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #57: @187 = @2[14] */ - for (rr=(&w187), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w187), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #58: @187 = (2.*@187) */ - w187 = (2.* w187 ); + w187 = (2. * w187); /* #59: @186 = (@187*@186) */ - w186 = (w187*w186); + w186 = (w187 * w186); /* #60: (@1[14] += @186) */ - for (rr=w1+14, ss=(&w186); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w186); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #61: @186 = @2[13] */ - for (rr=(&w186), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w186), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #62: @202 = (@186*@185) */ - w202 = (w186*w185); + w202 = (w186 * w185); /* #63: (@1[17] += @202) */ - for (rr=w1+17, ss=(&w202); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w202); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #64: @202 = @2[17] */ - for (rr=(&w202), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w202), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #65: @185 = (@202*@185) */ - w185 = (w202*w185); + w185 = (w202 * w185); /* #66: (@1[13] += @185) */ - for (rr=w1+13, ss=(&w185); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w185); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #67: @185 = @2[13] */ - for (rr=(&w185), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w185), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #68: @203 = (@185*@184) */ - w203 = (w185*w184); + w203 = (w185 * w184); /* #69: (@1[16] += @203) */ - for (rr=w1+16, ss=(&w203); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w203); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #70: @203 = @2[16] */ - for (rr=(&w203), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w203), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #71: @184 = (@203*@184) */ - w184 = (w203*w184); + w184 = (w203 * w184); /* #72: (@1[13] += @184) */ - for (rr=w1+13, ss=(&w184); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w184); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #73: @184 = @2[13] */ - for (rr=(&w184), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w184), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #74: @204 = (@184*@183) */ - w204 = (w184*w183); + w204 = (w184 * w183); /* #75: (@1[15] += @204) */ - for (rr=w1+15, ss=(&w204); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w204); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #76: @204 = @2[15] */ - for (rr=(&w204), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w204), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #77: @183 = (@204*@183) */ - w183 = (w204*w183); + w183 = (w204 * w183); /* #78: (@1[13] += @183) */ - for (rr=w1+13, ss=(&w183); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w183); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #79: @183 = @2[13] */ - for (rr=(&w183), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w183), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #80: @205 = (@183*@182) */ - w205 = (w183*w182); + w205 = (w183 * w182); /* #81: (@1[14] += @205) */ - for (rr=w1+14, ss=(&w205); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w205); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #82: @205 = @2[14] */ - for (rr=(&w205), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w205), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #83: @182 = (@205*@182) */ - w182 = (w205*w182); + w182 = (w205 * w182); /* #84: (@1[13] += @182) */ - for (rr=w1+13, ss=(&w182); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w182); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #85: @182 = @2[13] */ - for (rr=(&w182), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w182), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #86: @182 = (2.*@182) */ - w182 = (2.* w182 ); + w182 = (2. * w182); /* #87: @181 = (@182*@181) */ - w181 = (w182*w181); + w181 = (w182 * w181); /* #88: (@1[13] += @181) */ - for (rr=w1+13, ss=(&w181); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w181); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #89: @181 = @2[12] */ - for (rr=(&w181), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w181), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #90: @206 = (@181*@180) */ - w206 = (w181*w180); + w206 = (w181 * w180); /* #91: (@1[17] += @206) */ - for (rr=w1+17, ss=(&w206); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w206); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #92: @206 = @2[17] */ - for (rr=(&w206), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w206), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #93: @180 = (@206*@180) */ - w180 = (w206*w180); + w180 = (w206 * w180); /* #94: (@1[12] += @180) */ - for (rr=w1+12, ss=(&w180); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w180); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #95: @180 = @2[12] */ - for (rr=(&w180), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w180), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #96: @207 = (@180*@179) */ - w207 = (w180*w179); + w207 = (w180 * w179); /* #97: (@1[16] += @207) */ - for (rr=w1+16, ss=(&w207); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w207); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #98: @207 = @2[16] */ - for (rr=(&w207), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w207), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #99: @179 = (@207*@179) */ - w179 = (w207*w179); + w179 = (w207 * w179); /* #100: (@1[12] += @179) */ - for (rr=w1+12, ss=(&w179); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w179); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #101: @179 = @2[12] */ - for (rr=(&w179), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w179), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #102: @208 = (@179*@178) */ - w208 = (w179*w178); + w208 = (w179 * w178); /* #103: (@1[15] += @208) */ - for (rr=w1+15, ss=(&w208); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w208); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #104: @208 = @2[15] */ - for (rr=(&w208), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w208), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #105: @178 = (@208*@178) */ - w178 = (w208*w178); + w178 = (w208 * w178); /* #106: (@1[12] += @178) */ - for (rr=w1+12, ss=(&w178); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w178); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #107: @178 = @2[12] */ - for (rr=(&w178), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w178), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #108: @209 = (@178*@177) */ - w209 = (w178*w177); + w209 = (w178 * w177); /* #109: (@1[14] += @209) */ - for (rr=w1+14, ss=(&w209); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w209); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #110: @209 = @2[14] */ - for (rr=(&w209), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w209), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #111: @177 = (@209*@177) */ - w177 = (w209*w177); + w177 = (w209 * w177); /* #112: (@1[12] += @177) */ - for (rr=w1+12, ss=(&w177); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w177); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #113: @177 = @2[12] */ - for (rr=(&w177), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w177), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #114: @210 = (@177*@176) */ - w210 = (w177*w176); + w210 = (w177 * w176); /* #115: (@1[13] += @210) */ - for (rr=w1+13, ss=(&w210); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w210); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #116: @210 = @2[13] */ - for (rr=(&w210), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w210), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #117: @176 = (@210*@176) */ - w176 = (w210*w176); + w176 = (w210 * w176); /* #118: (@1[12] += @176) */ - for (rr=w1+12, ss=(&w176); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w176); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #119: @176 = @2[12] */ - for (rr=(&w176), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w176), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #120: @176 = (2.*@176) */ - w176 = (2.* w176 ); + w176 = (2. * w176); /* #121: @175 = (@176*@175) */ - w175 = (w176*w175); + w175 = (w176 * w175); /* #122: (@1[12] += @175) */ - for (rr=w1+12, ss=(&w175); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w175); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #123: @175 = @2[11] */ - for (rr=(&w175), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w175), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #124: @211 = (@175*@174) */ - w211 = (w175*w174); + w211 = (w175 * w174); /* #125: (@1[17] += @211) */ - for (rr=w1+17, ss=(&w211); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w211); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #126: @211 = @2[17] */ - for (rr=(&w211), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w211), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #127: @174 = (@211*@174) */ - w174 = (w211*w174); + w174 = (w211 * w174); /* #128: (@1[11] += @174) */ - for (rr=w1+11, ss=(&w174); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w174); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #129: @174 = @2[11] */ - for (rr=(&w174), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w174), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #130: @212 = (@174*@173) */ - w212 = (w174*w173); + w212 = (w174 * w173); /* #131: (@1[16] += @212) */ - for (rr=w1+16, ss=(&w212); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w212); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #132: @212 = @2[16] */ - for (rr=(&w212), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w212), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #133: @173 = (@212*@173) */ - w173 = (w212*w173); + w173 = (w212 * w173); /* #134: (@1[11] += @173) */ - for (rr=w1+11, ss=(&w173); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w173); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #135: @173 = @2[11] */ - for (rr=(&w173), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w173), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #136: @213 = (@173*@172) */ - w213 = (w173*w172); + w213 = (w173 * w172); /* #137: (@1[15] += @213) */ - for (rr=w1+15, ss=(&w213); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w213); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #138: @213 = @2[15] */ - for (rr=(&w213), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w213), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #139: @172 = (@213*@172) */ - w172 = (w213*w172); + w172 = (w213 * w172); /* #140: (@1[11] += @172) */ - for (rr=w1+11, ss=(&w172); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w172); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #141: @172 = @2[11] */ - for (rr=(&w172), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w172), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #142: @214 = (@172*@171) */ - w214 = (w172*w171); + w214 = (w172 * w171); /* #143: (@1[14] += @214) */ - for (rr=w1+14, ss=(&w214); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w214); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #144: @214 = @2[14] */ - for (rr=(&w214), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w214), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #145: @171 = (@214*@171) */ - w171 = (w214*w171); + w171 = (w214 * w171); /* #146: (@1[11] += @171) */ - for (rr=w1+11, ss=(&w171); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w171); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #147: @171 = @2[11] */ - for (rr=(&w171), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w171), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #148: @215 = (@171*@170) */ - w215 = (w171*w170); + w215 = (w171 * w170); /* #149: (@1[13] += @215) */ - for (rr=w1+13, ss=(&w215); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w215); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #150: @215 = @2[13] */ - for (rr=(&w215), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w215), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #151: @170 = (@215*@170) */ - w170 = (w215*w170); + w170 = (w215 * w170); /* #152: (@1[11] += @170) */ - for (rr=w1+11, ss=(&w170); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w170); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #153: @170 = @2[11] */ - for (rr=(&w170), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w170), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #154: @216 = (@170*@169) */ - w216 = (w170*w169); + w216 = (w170 * w169); /* #155: (@1[12] += @216) */ - for (rr=w1+12, ss=(&w216); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w216); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #156: @216 = @2[12] */ - for (rr=(&w216), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w216), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #157: @169 = (@216*@169) */ - w169 = (w216*w169); + w169 = (w216 * w169); /* #158: (@1[11] += @169) */ - for (rr=w1+11, ss=(&w169); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w169); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #159: @169 = @2[11] */ - for (rr=(&w169), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w169), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #160: @169 = (2.*@169) */ - w169 = (2.* w169 ); + w169 = (2. * w169); /* #161: @168 = (@169*@168) */ - w168 = (w169*w168); + w168 = (w169 * w168); /* #162: (@1[11] += @168) */ - for (rr=w1+11, ss=(&w168); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w168); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #163: @168 = @2[10] */ - for (rr=(&w168), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w168), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #164: @217 = (@168*@167) */ - w217 = (w168*w167); + w217 = (w168 * w167); /* #165: (@1[17] += @217) */ - for (rr=w1+17, ss=(&w217); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w217); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #166: @217 = @2[17] */ - for (rr=(&w217), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w217), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #167: @167 = (@217*@167) */ - w167 = (w217*w167); + w167 = (w217 * w167); /* #168: (@1[10] += @167) */ - for (rr=w1+10, ss=(&w167); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w167); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #169: @167 = @2[10] */ - for (rr=(&w167), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w167), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #170: @218 = (@167*@166) */ - w218 = (w167*w166); + w218 = (w167 * w166); /* #171: (@1[16] += @218) */ - for (rr=w1+16, ss=(&w218); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w218); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #172: @218 = @2[16] */ - for (rr=(&w218), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w218), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #173: @166 = (@218*@166) */ - w166 = (w218*w166); + w166 = (w218 * w166); /* #174: (@1[10] += @166) */ - for (rr=w1+10, ss=(&w166); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w166); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #175: @166 = @2[10] */ - for (rr=(&w166), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w166), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #176: @219 = (@166*@165) */ - w219 = (w166*w165); + w219 = (w166 * w165); /* #177: (@1[15] += @219) */ - for (rr=w1+15, ss=(&w219); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w219); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #178: @219 = @2[15] */ - for (rr=(&w219), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w219), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #179: @165 = (@219*@165) */ - w165 = (w219*w165); + w165 = (w219 * w165); /* #180: (@1[10] += @165) */ - for (rr=w1+10, ss=(&w165); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w165); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #181: @165 = @2[10] */ - for (rr=(&w165), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w165), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #182: @220 = (@165*@164) */ - w220 = (w165*w164); + w220 = (w165 * w164); /* #183: (@1[14] += @220) */ - for (rr=w1+14, ss=(&w220); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w220); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #184: @220 = @2[14] */ - for (rr=(&w220), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w220), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #185: @164 = (@220*@164) */ - w164 = (w220*w164); + w164 = (w220 * w164); /* #186: (@1[10] += @164) */ - for (rr=w1+10, ss=(&w164); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w164); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #187: @164 = @2[10] */ - for (rr=(&w164), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w164), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #188: @221 = (@164*@163) */ - w221 = (w164*w163); + w221 = (w164 * w163); /* #189: (@1[13] += @221) */ - for (rr=w1+13, ss=(&w221); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w221); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #190: @221 = @2[13] */ - for (rr=(&w221), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w221), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #191: @163 = (@221*@163) */ - w163 = (w221*w163); + w163 = (w221 * w163); /* #192: (@1[10] += @163) */ - for (rr=w1+10, ss=(&w163); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w163); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #193: @163 = @2[10] */ - for (rr=(&w163), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w163), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #194: @222 = (@163*@162) */ - w222 = (w163*w162); + w222 = (w163 * w162); /* #195: (@1[12] += @222) */ - for (rr=w1+12, ss=(&w222); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w222); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #196: @222 = @2[12] */ - for (rr=(&w222), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w222), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #197: @162 = (@222*@162) */ - w162 = (w222*w162); + w162 = (w222 * w162); /* #198: (@1[10] += @162) */ - for (rr=w1+10, ss=(&w162); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w162); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #199: @162 = @2[10] */ - for (rr=(&w162), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w162), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #200: @223 = (@162*@161) */ - w223 = (w162*w161); + w223 = (w162 * w161); /* #201: (@1[11] += @223) */ - for (rr=w1+11, ss=(&w223); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w223); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #202: @223 = @2[11] */ - for (rr=(&w223), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w223), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #203: @161 = (@223*@161) */ - w161 = (w223*w161); + w161 = (w223 * w161); /* #204: (@1[10] += @161) */ - for (rr=w1+10, ss=(&w161); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w161); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #205: @161 = @2[10] */ - for (rr=(&w161), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w161), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #206: @161 = (2.*@161) */ - w161 = (2.* w161 ); + w161 = (2. * w161); /* #207: @160 = (@161*@160) */ - w160 = (w161*w160); + w160 = (w161 * w160); /* #208: (@1[10] += @160) */ - for (rr=w1+10, ss=(&w160); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w160); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #209: @160 = @2[9] */ - for (rr=(&w160), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w160), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #210: @224 = (@160*@159) */ - w224 = (w160*w159); + w224 = (w160 * w159); /* #211: (@1[17] += @224) */ - for (rr=w1+17, ss=(&w224); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w224); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #212: @224 = @2[17] */ - for (rr=(&w224), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w224), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #213: @159 = (@224*@159) */ - w159 = (w224*w159); + w159 = (w224 * w159); /* #214: (@1[9] += @159) */ - for (rr=w1+9, ss=(&w159); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w159); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #215: @159 = @2[9] */ - for (rr=(&w159), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w159), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #216: @225 = (@159*@158) */ - w225 = (w159*w158); + w225 = (w159 * w158); /* #217: (@1[16] += @225) */ - for (rr=w1+16, ss=(&w225); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w225); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #218: @225 = @2[16] */ - for (rr=(&w225), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w225), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #219: @158 = (@225*@158) */ - w158 = (w225*w158); + w158 = (w225 * w158); /* #220: (@1[9] += @158) */ - for (rr=w1+9, ss=(&w158); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w158); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #221: @158 = @2[9] */ - for (rr=(&w158), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w158), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #222: @226 = (@158*@157) */ - w226 = (w158*w157); + w226 = (w158 * w157); /* #223: (@1[15] += @226) */ - for (rr=w1+15, ss=(&w226); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w226); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #224: @226 = @2[15] */ - for (rr=(&w226), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w226), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #225: @157 = (@226*@157) */ - w157 = (w226*w157); + w157 = (w226 * w157); /* #226: (@1[9] += @157) */ - for (rr=w1+9, ss=(&w157); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w157); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #227: @157 = @2[9] */ - for (rr=(&w157), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w157), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #228: @227 = (@157*@156) */ - w227 = (w157*w156); + w227 = (w157 * w156); /* #229: (@1[14] += @227) */ - for (rr=w1+14, ss=(&w227); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w227); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #230: @227 = @2[14] */ - for (rr=(&w227), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w227), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #231: @156 = (@227*@156) */ - w156 = (w227*w156); + w156 = (w227 * w156); /* #232: (@1[9] += @156) */ - for (rr=w1+9, ss=(&w156); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w156); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #233: @156 = @2[9] */ - for (rr=(&w156), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w156), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #234: @228 = (@156*@155) */ - w228 = (w156*w155); + w228 = (w156 * w155); /* #235: (@1[13] += @228) */ - for (rr=w1+13, ss=(&w228); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w228); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #236: @228 = @2[13] */ - for (rr=(&w228), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w228), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #237: @155 = (@228*@155) */ - w155 = (w228*w155); + w155 = (w228 * w155); /* #238: (@1[9] += @155) */ - for (rr=w1+9, ss=(&w155); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w155); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #239: @155 = @2[9] */ - for (rr=(&w155), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w155), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #240: @229 = (@155*@154) */ - w229 = (w155*w154); + w229 = (w155 * w154); /* #241: (@1[12] += @229) */ - for (rr=w1+12, ss=(&w229); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w229); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #242: @229 = @2[12] */ - for (rr=(&w229), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w229), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #243: @154 = (@229*@154) */ - w154 = (w229*w154); + w154 = (w229 * w154); /* #244: (@1[9] += @154) */ - for (rr=w1+9, ss=(&w154); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w154); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #245: @154 = @2[9] */ - for (rr=(&w154), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w154), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #246: @230 = (@154*@153) */ - w230 = (w154*w153); + w230 = (w154 * w153); /* #247: (@1[11] += @230) */ - for (rr=w1+11, ss=(&w230); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w230); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #248: @230 = @2[11] */ - for (rr=(&w230), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w230), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #249: @153 = (@230*@153) */ - w153 = (w230*w153); + w153 = (w230 * w153); /* #250: (@1[9] += @153) */ - for (rr=w1+9, ss=(&w153); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w153); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #251: @153 = @2[9] */ - for (rr=(&w153), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w153), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #252: @231 = (@153*@152) */ - w231 = (w153*w152); + w231 = (w153 * w152); /* #253: (@1[10] += @231) */ - for (rr=w1+10, ss=(&w231); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w231); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #254: @231 = @2[10] */ - for (rr=(&w231), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w231), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #255: @152 = (@231*@152) */ - w152 = (w231*w152); + w152 = (w231 * w152); /* #256: (@1[9] += @152) */ - for (rr=w1+9, ss=(&w152); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w152); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #257: @152 = @2[9] */ - for (rr=(&w152), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w152), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #258: @152 = (2.*@152) */ - w152 = (2.* w152 ); + w152 = (2. * w152); /* #259: @151 = (@152*@151) */ - w151 = (w152*w151); + w151 = (w152 * w151); /* #260: (@1[9] += @151) */ - for (rr=w1+9, ss=(&w151); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w151); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #261: @151 = @2[8] */ - for (rr=(&w151), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w151), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #262: @232 = (@151*@150) */ - w232 = (w151*w150); + w232 = (w151 * w150); /* #263: (@1[17] += @232) */ - for (rr=w1+17, ss=(&w232); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w232); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #264: @232 = @2[17] */ - for (rr=(&w232), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w232), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #265: @150 = (@232*@150) */ - w150 = (w232*w150); + w150 = (w232 * w150); /* #266: (@1[8] += @150) */ - for (rr=w1+8, ss=(&w150); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w150); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #267: @150 = @2[8] */ - for (rr=(&w150), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w150), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #268: @233 = (@150*@149) */ - w233 = (w150*w149); + w233 = (w150 * w149); /* #269: (@1[16] += @233) */ - for (rr=w1+16, ss=(&w233); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w233); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #270: @233 = @2[16] */ - for (rr=(&w233), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w233), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #271: @149 = (@233*@149) */ - w149 = (w233*w149); + w149 = (w233 * w149); /* #272: (@1[8] += @149) */ - for (rr=w1+8, ss=(&w149); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w149); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #273: @149 = @2[8] */ - for (rr=(&w149), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w149), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #274: @234 = (@149*@148) */ - w234 = (w149*w148); + w234 = (w149 * w148); /* #275: (@1[15] += @234) */ - for (rr=w1+15, ss=(&w234); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w234); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #276: @234 = @2[15] */ - for (rr=(&w234), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w234), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #277: @148 = (@234*@148) */ - w148 = (w234*w148); + w148 = (w234 * w148); /* #278: (@1[8] += @148) */ - for (rr=w1+8, ss=(&w148); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w148); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #279: @148 = @2[8] */ - for (rr=(&w148), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w148), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #280: @235 = (@148*@147) */ - w235 = (w148*w147); + w235 = (w148 * w147); /* #281: (@1[14] += @235) */ - for (rr=w1+14, ss=(&w235); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w235); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #282: @235 = @2[14] */ - for (rr=(&w235), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w235), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #283: @147 = (@235*@147) */ - w147 = (w235*w147); + w147 = (w235 * w147); /* #284: (@1[8] += @147) */ - for (rr=w1+8, ss=(&w147); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w147); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #285: @147 = @2[8] */ - for (rr=(&w147), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w147), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #286: @236 = (@147*@146) */ - w236 = (w147*w146); + w236 = (w147 * w146); /* #287: (@1[13] += @236) */ - for (rr=w1+13, ss=(&w236); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w236); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #288: @236 = @2[13] */ - for (rr=(&w236), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w236), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #289: @146 = (@236*@146) */ - w146 = (w236*w146); + w146 = (w236 * w146); /* #290: (@1[8] += @146) */ - for (rr=w1+8, ss=(&w146); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w146); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #291: @146 = @2[8] */ - for (rr=(&w146), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w146), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #292: @237 = (@146*@145) */ - w237 = (w146*w145); + w237 = (w146 * w145); /* #293: (@1[12] += @237) */ - for (rr=w1+12, ss=(&w237); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w237); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #294: @237 = @2[12] */ - for (rr=(&w237), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w237), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #295: @145 = (@237*@145) */ - w145 = (w237*w145); + w145 = (w237 * w145); /* #296: (@1[8] += @145) */ - for (rr=w1+8, ss=(&w145); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w145); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #297: @145 = @2[8] */ - for (rr=(&w145), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w145), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #298: @238 = (@145*@144) */ - w238 = (w145*w144); + w238 = (w145 * w144); /* #299: (@1[11] += @238) */ - for (rr=w1+11, ss=(&w238); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w238); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #300: @238 = @2[11] */ - for (rr=(&w238), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w238), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #301: @144 = (@238*@144) */ - w144 = (w238*w144); + w144 = (w238 * w144); /* #302: (@1[8] += @144) */ - for (rr=w1+8, ss=(&w144); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w144); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #303: @144 = @2[8] */ - for (rr=(&w144), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w144), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #304: @239 = (@144*@143) */ - w239 = (w144*w143); + w239 = (w144 * w143); /* #305: (@1[10] += @239) */ - for (rr=w1+10, ss=(&w239); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w239); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #306: @239 = @2[10] */ - for (rr=(&w239), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w239), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #307: @143 = (@239*@143) */ - w143 = (w239*w143); + w143 = (w239 * w143); /* #308: (@1[8] += @143) */ - for (rr=w1+8, ss=(&w143); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w143); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #309: @143 = @2[8] */ - for (rr=(&w143), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w143), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #310: @240 = (@143*@142) */ - w240 = (w143*w142); + w240 = (w143 * w142); /* #311: (@1[9] += @240) */ - for (rr=w1+9, ss=(&w240); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w240); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #312: @240 = @2[9] */ - for (rr=(&w240), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w240), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #313: @142 = (@240*@142) */ - w142 = (w240*w142); + w142 = (w240 * w142); /* #314: (@1[8] += @142) */ - for (rr=w1+8, ss=(&w142); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w142); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #315: @142 = @2[8] */ - for (rr=(&w142), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w142), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #316: @142 = (2.*@142) */ - w142 = (2.* w142 ); + w142 = (2. * w142); /* #317: @141 = (@142*@141) */ - w141 = (w142*w141); + w141 = (w142 * w141); /* #318: (@1[8] += @141) */ - for (rr=w1+8, ss=(&w141); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w141); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #319: @141 = @2[7] */ - for (rr=(&w141), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w141), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #320: @241 = (@141*@140) */ - w241 = (w141*w140); + w241 = (w141 * w140); /* #321: (@1[17] += @241) */ - for (rr=w1+17, ss=(&w241); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w241); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #322: @241 = @2[17] */ - for (rr=(&w241), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w241), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #323: @140 = (@241*@140) */ - w140 = (w241*w140); + w140 = (w241 * w140); /* #324: (@1[7] += @140) */ - for (rr=w1+7, ss=(&w140); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w140); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #325: @140 = @2[7] */ - for (rr=(&w140), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w140), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #326: @242 = (@140*@139) */ - w242 = (w140*w139); + w242 = (w140 * w139); /* #327: (@1[16] += @242) */ - for (rr=w1+16, ss=(&w242); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w242); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #328: @242 = @2[16] */ - for (rr=(&w242), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w242), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #329: @139 = (@242*@139) */ - w139 = (w242*w139); + w139 = (w242 * w139); /* #330: (@1[7] += @139) */ - for (rr=w1+7, ss=(&w139); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w139); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #331: @139 = @2[7] */ - for (rr=(&w139), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w139), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #332: @243 = (@139*@138) */ - w243 = (w139*w138); + w243 = (w139 * w138); /* #333: (@1[15] += @243) */ - for (rr=w1+15, ss=(&w243); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w243); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #334: @243 = @2[15] */ - for (rr=(&w243), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w243), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #335: @138 = (@243*@138) */ - w138 = (w243*w138); + w138 = (w243 * w138); /* #336: (@1[7] += @138) */ - for (rr=w1+7, ss=(&w138); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w138); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #337: @138 = @2[7] */ - for (rr=(&w138), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w138), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #338: @244 = (@138*@137) */ - w244 = (w138*w137); + w244 = (w138 * w137); /* #339: (@1[14] += @244) */ - for (rr=w1+14, ss=(&w244); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w244); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #340: @244 = @2[14] */ - for (rr=(&w244), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w244), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #341: @137 = (@244*@137) */ - w137 = (w244*w137); + w137 = (w244 * w137); /* #342: (@1[7] += @137) */ - for (rr=w1+7, ss=(&w137); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w137); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #343: @137 = @2[7] */ - for (rr=(&w137), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w137), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #344: @245 = (@137*@136) */ - w245 = (w137*w136); + w245 = (w137 * w136); /* #345: (@1[13] += @245) */ - for (rr=w1+13, ss=(&w245); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w245); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #346: @245 = @2[13] */ - for (rr=(&w245), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w245), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #347: @136 = (@245*@136) */ - w136 = (w245*w136); + w136 = (w245 * w136); /* #348: (@1[7] += @136) */ - for (rr=w1+7, ss=(&w136); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w136); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #349: @136 = @2[7] */ - for (rr=(&w136), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w136), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #350: @246 = (@136*@135) */ - w246 = (w136*w135); + w246 = (w136 * w135); /* #351: (@1[12] += @246) */ - for (rr=w1+12, ss=(&w246); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w246); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #352: @246 = @2[12] */ - for (rr=(&w246), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w246), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #353: @135 = (@246*@135) */ - w135 = (w246*w135); + w135 = (w246 * w135); /* #354: (@1[7] += @135) */ - for (rr=w1+7, ss=(&w135); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w135); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #355: @135 = @2[7] */ - for (rr=(&w135), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w135), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #356: @247 = (@135*@134) */ - w247 = (w135*w134); + w247 = (w135 * w134); /* #357: (@1[11] += @247) */ - for (rr=w1+11, ss=(&w247); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w247); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #358: @247 = @2[11] */ - for (rr=(&w247), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w247), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #359: @134 = (@247*@134) */ - w134 = (w247*w134); + w134 = (w247 * w134); /* #360: (@1[7] += @134) */ - for (rr=w1+7, ss=(&w134); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w134); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #361: @134 = @2[7] */ - for (rr=(&w134), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w134), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #362: @248 = (@134*@133) */ - w248 = (w134*w133); + w248 = (w134 * w133); /* #363: (@1[10] += @248) */ - for (rr=w1+10, ss=(&w248); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w248); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #364: @248 = @2[10] */ - for (rr=(&w248), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w248), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #365: @133 = (@248*@133) */ - w133 = (w248*w133); + w133 = (w248 * w133); /* #366: (@1[7] += @133) */ - for (rr=w1+7, ss=(&w133); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w133); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #367: @133 = @2[7] */ - for (rr=(&w133), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w133), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #368: @249 = (@133*@132) */ - w249 = (w133*w132); + w249 = (w133 * w132); /* #369: (@1[9] += @249) */ - for (rr=w1+9, ss=(&w249); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w249); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #370: @249 = @2[9] */ - for (rr=(&w249), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w249), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #371: @132 = (@249*@132) */ - w132 = (w249*w132); + w132 = (w249 * w132); /* #372: (@1[7] += @132) */ - for (rr=w1+7, ss=(&w132); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w132); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #373: @132 = @2[7] */ - for (rr=(&w132), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w132), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #374: @250 = (@132*@131) */ - w250 = (w132*w131); + w250 = (w132 * w131); /* #375: (@1[8] += @250) */ - for (rr=w1+8, ss=(&w250); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w250); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #376: @250 = @2[8] */ - for (rr=(&w250), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w250), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #377: @131 = (@250*@131) */ - w131 = (w250*w131); + w131 = (w250 * w131); /* #378: (@1[7] += @131) */ - for (rr=w1+7, ss=(&w131); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w131); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #379: @131 = @2[7] */ - for (rr=(&w131), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w131), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #380: @131 = (2.*@131) */ - w131 = (2.* w131 ); + w131 = (2. * w131); /* #381: @130 = (@131*@130) */ - w130 = (w131*w130); + w130 = (w131 * w130); /* #382: (@1[7] += @130) */ - for (rr=w1+7, ss=(&w130); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w130); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #383: @130 = @2[6] */ - for (rr=(&w130), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w130), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #384: @251 = (@130*@129) */ - w251 = (w130*w129); + w251 = (w130 * w129); /* #385: (@1[17] += @251) */ - for (rr=w1+17, ss=(&w251); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w251); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #386: @251 = @2[17] */ - for (rr=(&w251), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w251), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #387: @129 = (@251*@129) */ - w129 = (w251*w129); + w129 = (w251 * w129); /* #388: (@1[6] += @129) */ - for (rr=w1+6, ss=(&w129); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w129); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #389: @129 = @2[6] */ - for (rr=(&w129), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w129), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #390: @252 = (@129*@128) */ - w252 = (w129*w128); + w252 = (w129 * w128); /* #391: (@1[16] += @252) */ - for (rr=w1+16, ss=(&w252); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w252); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #392: @252 = @2[16] */ - for (rr=(&w252), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w252), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #393: @128 = (@252*@128) */ - w128 = (w252*w128); + w128 = (w252 * w128); /* #394: (@1[6] += @128) */ - for (rr=w1+6, ss=(&w128); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w128); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #395: @128 = @2[6] */ - for (rr=(&w128), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w128), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #396: @253 = (@128*@127) */ - w253 = (w128*w127); + w253 = (w128 * w127); /* #397: (@1[15] += @253) */ - for (rr=w1+15, ss=(&w253); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w253); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #398: @253 = @2[15] */ - for (rr=(&w253), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w253), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #399: @127 = (@253*@127) */ - w127 = (w253*w127); + w127 = (w253 * w127); /* #400: (@1[6] += @127) */ - for (rr=w1+6, ss=(&w127); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w127); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #401: @127 = @2[6] */ - for (rr=(&w127), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w127), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #402: @254 = (@127*@126) */ - w254 = (w127*w126); + w254 = (w127 * w126); /* #403: (@1[14] += @254) */ - for (rr=w1+14, ss=(&w254); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w254); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #404: @254 = @2[14] */ - for (rr=(&w254), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w254), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #405: @126 = (@254*@126) */ - w126 = (w254*w126); + w126 = (w254 * w126); /* #406: (@1[6] += @126) */ - for (rr=w1+6, ss=(&w126); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w126); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #407: @126 = @2[6] */ - for (rr=(&w126), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w126), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #408: @255 = (@126*@125) */ - w255 = (w126*w125); + w255 = (w126 * w125); /* #409: (@1[13] += @255) */ - for (rr=w1+13, ss=(&w255); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w255); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #410: @255 = @2[13] */ - for (rr=(&w255), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w255), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #411: @125 = (@255*@125) */ - w125 = (w255*w125); + w125 = (w255 * w125); /* #412: (@1[6] += @125) */ - for (rr=w1+6, ss=(&w125); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w125); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #413: @125 = @2[6] */ - for (rr=(&w125), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w125), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #414: @256 = (@125*@124) */ - w256 = (w125*w124); + w256 = (w125 * w124); /* #415: (@1[12] += @256) */ - for (rr=w1+12, ss=(&w256); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w256); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #416: @256 = @2[12] */ - for (rr=(&w256), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w256), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #417: @124 = (@256*@124) */ - w124 = (w256*w124); + w124 = (w256 * w124); /* #418: (@1[6] += @124) */ - for (rr=w1+6, ss=(&w124); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w124); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #419: @124 = @2[6] */ - for (rr=(&w124), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w124), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #420: @257 = (@124*@123) */ - w257 = (w124*w123); + w257 = (w124 * w123); /* #421: (@1[11] += @257) */ - for (rr=w1+11, ss=(&w257); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w257); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #422: @257 = @2[11] */ - for (rr=(&w257), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w257), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #423: @123 = (@257*@123) */ - w123 = (w257*w123); + w123 = (w257 * w123); /* #424: (@1[6] += @123) */ - for (rr=w1+6, ss=(&w123); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w123); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #425: @123 = @2[6] */ - for (rr=(&w123), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w123), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #426: @258 = (@123*@122) */ - w258 = (w123*w122); + w258 = (w123 * w122); /* #427: (@1[10] += @258) */ - for (rr=w1+10, ss=(&w258); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w258); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #428: @258 = @2[10] */ - for (rr=(&w258), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w258), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #429: @122 = (@258*@122) */ - w122 = (w258*w122); + w122 = (w258 * w122); /* #430: (@1[6] += @122) */ - for (rr=w1+6, ss=(&w122); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w122); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #431: @122 = @2[6] */ - for (rr=(&w122), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w122), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #432: @259 = (@122*@121) */ - w259 = (w122*w121); + w259 = (w122 * w121); /* #433: (@1[9] += @259) */ - for (rr=w1+9, ss=(&w259); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w259); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #434: @259 = @2[9] */ - for (rr=(&w259), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w259), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #435: @121 = (@259*@121) */ - w121 = (w259*w121); + w121 = (w259 * w121); /* #436: (@1[6] += @121) */ - for (rr=w1+6, ss=(&w121); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w121); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #437: @121 = @2[6] */ - for (rr=(&w121), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w121), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #438: @260 = (@121*@120) */ - w260 = (w121*w120); + w260 = (w121 * w120); /* #439: (@1[8] += @260) */ - for (rr=w1+8, ss=(&w260); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w260); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #440: @260 = @2[8] */ - for (rr=(&w260), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w260), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #441: @120 = (@260*@120) */ - w120 = (w260*w120); + w120 = (w260 * w120); /* #442: (@1[6] += @120) */ - for (rr=w1+6, ss=(&w120); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w120); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #443: @120 = @2[6] */ - for (rr=(&w120), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w120), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #444: @261 = (@120*@119) */ - w261 = (w120*w119); + w261 = (w120 * w119); /* #445: (@1[7] += @261) */ - for (rr=w1+7, ss=(&w261); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w261); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #446: @261 = @2[7] */ - for (rr=(&w261), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w261), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #447: @119 = (@261*@119) */ - w119 = (w261*w119); + w119 = (w261 * w119); /* #448: (@1[6] += @119) */ - for (rr=w1+6, ss=(&w119); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w119); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #449: @119 = @2[6] */ - for (rr=(&w119), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w119), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #450: @119 = (2.*@119) */ - w119 = (2.* w119 ); + w119 = (2. * w119); /* #451: @118 = (@119*@118) */ - w118 = (w119*w118); + w118 = (w119 * w118); /* #452: (@1[6] += @118) */ - for (rr=w1+6, ss=(&w118); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w118); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #453: @118 = @2[5] */ - for (rr=(&w118), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w118), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #454: @262 = (@118*@117) */ - w262 = (w118*w117); + w262 = (w118 * w117); /* #455: (@1[17] += @262) */ - for (rr=w1+17, ss=(&w262); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w262); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #456: @262 = @2[17] */ - for (rr=(&w262), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w262), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #457: @117 = (@262*@117) */ - w117 = (w262*w117); + w117 = (w262 * w117); /* #458: (@1[5] += @117) */ - for (rr=w1+5, ss=(&w117); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w117); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #459: @117 = @2[5] */ - for (rr=(&w117), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w117), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #460: @263 = (@117*@116) */ - w263 = (w117*w116); + w263 = (w117 * w116); /* #461: (@1[16] += @263) */ - for (rr=w1+16, ss=(&w263); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w263); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #462: @263 = @2[16] */ - for (rr=(&w263), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w263), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #463: @116 = (@263*@116) */ - w116 = (w263*w116); + w116 = (w263 * w116); /* #464: (@1[5] += @116) */ - for (rr=w1+5, ss=(&w116); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w116); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #465: @116 = @2[5] */ - for (rr=(&w116), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w116), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #466: @264 = (@116*@115) */ - w264 = (w116*w115); + w264 = (w116 * w115); /* #467: (@1[15] += @264) */ - for (rr=w1+15, ss=(&w264); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w264); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #468: @264 = @2[15] */ - for (rr=(&w264), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w264), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #469: @115 = (@264*@115) */ - w115 = (w264*w115); + w115 = (w264 * w115); /* #470: (@1[5] += @115) */ - for (rr=w1+5, ss=(&w115); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w115); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #471: @115 = @2[5] */ - for (rr=(&w115), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w115), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #472: @265 = (@115*@114) */ - w265 = (w115*w114); + w265 = (w115 * w114); /* #473: (@1[14] += @265) */ - for (rr=w1+14, ss=(&w265); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w265); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #474: @265 = @2[14] */ - for (rr=(&w265), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w265), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #475: @114 = (@265*@114) */ - w114 = (w265*w114); + w114 = (w265 * w114); /* #476: (@1[5] += @114) */ - for (rr=w1+5, ss=(&w114); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w114); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #477: @114 = @2[5] */ - for (rr=(&w114), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w114), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #478: @266 = (@114*@113) */ - w266 = (w114*w113); + w266 = (w114 * w113); /* #479: (@1[13] += @266) */ - for (rr=w1+13, ss=(&w266); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w266); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #480: @266 = @2[13] */ - for (rr=(&w266), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w266), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #481: @113 = (@266*@113) */ - w113 = (w266*w113); + w113 = (w266 * w113); /* #482: (@1[5] += @113) */ - for (rr=w1+5, ss=(&w113); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w113); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #483: @113 = @2[5] */ - for (rr=(&w113), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w113), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #484: @267 = (@113*@112) */ - w267 = (w113*w112); + w267 = (w113 * w112); /* #485: (@1[12] += @267) */ - for (rr=w1+12, ss=(&w267); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w267); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #486: @267 = @2[12] */ - for (rr=(&w267), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w267), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #487: @112 = (@267*@112) */ - w112 = (w267*w112); + w112 = (w267 * w112); /* #488: (@1[5] += @112) */ - for (rr=w1+5, ss=(&w112); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w112); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #489: @112 = @2[5] */ - for (rr=(&w112), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w112), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #490: @268 = (@112*@111) */ - w268 = (w112*w111); + w268 = (w112 * w111); /* #491: (@1[11] += @268) */ - for (rr=w1+11, ss=(&w268); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w268); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #492: @268 = @2[11] */ - for (rr=(&w268), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w268), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #493: @111 = (@268*@111) */ - w111 = (w268*w111); + w111 = (w268 * w111); /* #494: (@1[5] += @111) */ - for (rr=w1+5, ss=(&w111); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w111); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #495: @111 = @2[5] */ - for (rr=(&w111), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w111), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #496: @269 = (@111*@110) */ - w269 = (w111*w110); + w269 = (w111 * w110); /* #497: (@1[10] += @269) */ - for (rr=w1+10, ss=(&w269); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w269); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #498: @269 = @2[10] */ - for (rr=(&w269), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w269), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #499: @110 = (@269*@110) */ - w110 = (w269*w110); + w110 = (w269 * w110); /* #500: (@1[5] += @110) */ - for (rr=w1+5, ss=(&w110); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w110); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #501: @110 = @2[5] */ - for (rr=(&w110), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w110), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #502: @270 = (@110*@109) */ - w270 = (w110*w109); + w270 = (w110 * w109); /* #503: (@1[9] += @270) */ - for (rr=w1+9, ss=(&w270); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w270); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #504: @270 = @2[9] */ - for (rr=(&w270), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w270), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #505: @109 = (@270*@109) */ - w109 = (w270*w109); + w109 = (w270 * w109); /* #506: (@1[5] += @109) */ - for (rr=w1+5, ss=(&w109); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w109); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #507: @109 = @2[5] */ - for (rr=(&w109), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w109), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #508: @271 = (@109*@108) */ - w271 = (w109*w108); + w271 = (w109 * w108); /* #509: (@1[8] += @271) */ - for (rr=w1+8, ss=(&w271); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w271); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #510: @271 = @2[8] */ - for (rr=(&w271), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w271), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #511: @108 = (@271*@108) */ - w108 = (w271*w108); + w108 = (w271 * w108); /* #512: (@1[5] += @108) */ - for (rr=w1+5, ss=(&w108); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w108); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #513: @108 = @2[5] */ - for (rr=(&w108), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w108), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #514: @272 = (@108*@107) */ - w272 = (w108*w107); + w272 = (w108 * w107); /* #515: (@1[7] += @272) */ - for (rr=w1+7, ss=(&w272); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w272); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #516: @272 = @2[7] */ - for (rr=(&w272), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w272), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #517: @107 = (@272*@107) */ - w107 = (w272*w107); + w107 = (w272 * w107); /* #518: (@1[5] += @107) */ - for (rr=w1+5, ss=(&w107); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w107); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #519: @107 = @2[5] */ - for (rr=(&w107), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w107), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #520: @273 = (@107*@106) */ - w273 = (w107*w106); + w273 = (w107 * w106); /* #521: (@1[6] += @273) */ - for (rr=w1+6, ss=(&w273); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w273); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #522: @273 = @2[6] */ - for (rr=(&w273), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w273), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #523: @106 = (@273*@106) */ - w106 = (w273*w106); + w106 = (w273 * w106); /* #524: (@1[5] += @106) */ - for (rr=w1+5, ss=(&w106); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w106); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #525: @106 = @2[5] */ - for (rr=(&w106), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w106), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #526: @106 = (2.*@106) */ - w106 = (2.* w106 ); + w106 = (2. * w106); /* #527: @105 = (@106*@105) */ - w105 = (w106*w105); + w105 = (w106 * w105); /* #528: (@1[5] += @105) */ - for (rr=w1+5, ss=(&w105); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w105); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #529: @105 = @2[4] */ - for (rr=(&w105), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w105), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #530: @274 = (@105*@104) */ - w274 = (w105*w104); + w274 = (w105 * w104); /* #531: (@1[17] += @274) */ - for (rr=w1+17, ss=(&w274); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w274); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #532: @274 = @2[17] */ - for (rr=(&w274), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w274), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #533: @104 = (@274*@104) */ - w104 = (w274*w104); + w104 = (w274 * w104); /* #534: (@1[4] += @104) */ - for (rr=w1+4, ss=(&w104); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w104); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #535: @104 = @2[4] */ - for (rr=(&w104), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w104), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #536: @275 = (@104*@103) */ - w275 = (w104*w103); + w275 = (w104 * w103); /* #537: (@1[16] += @275) */ - for (rr=w1+16, ss=(&w275); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w275); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #538: @275 = @2[16] */ - for (rr=(&w275), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w275), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #539: @103 = (@275*@103) */ - w103 = (w275*w103); + w103 = (w275 * w103); /* #540: (@1[4] += @103) */ - for (rr=w1+4, ss=(&w103); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w103); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #541: @103 = @2[4] */ - for (rr=(&w103), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w103), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #542: @276 = (@103*@102) */ - w276 = (w103*w102); + w276 = (w103 * w102); /* #543: (@1[15] += @276) */ - for (rr=w1+15, ss=(&w276); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w276); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #544: @276 = @2[15] */ - for (rr=(&w276), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w276), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #545: @102 = (@276*@102) */ - w102 = (w276*w102); + w102 = (w276 * w102); /* #546: (@1[4] += @102) */ - for (rr=w1+4, ss=(&w102); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w102); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #547: @102 = @2[4] */ - for (rr=(&w102), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w102), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #548: @277 = (@102*@101) */ - w277 = (w102*w101); + w277 = (w102 * w101); /* #549: (@1[14] += @277) */ - for (rr=w1+14, ss=(&w277); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w277); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #550: @277 = @2[14] */ - for (rr=(&w277), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w277), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #551: @101 = (@277*@101) */ - w101 = (w277*w101); + w101 = (w277 * w101); /* #552: (@1[4] += @101) */ - for (rr=w1+4, ss=(&w101); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w101); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #553: @101 = @2[4] */ - for (rr=(&w101), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w101), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #554: @278 = (@101*@100) */ - w278 = (w101*w100); + w278 = (w101 * w100); /* #555: (@1[13] += @278) */ - for (rr=w1+13, ss=(&w278); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w278); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #556: @278 = @2[13] */ - for (rr=(&w278), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w278), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #557: @100 = (@278*@100) */ - w100 = (w278*w100); + w100 = (w278 * w100); /* #558: (@1[4] += @100) */ - for (rr=w1+4, ss=(&w100); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w100); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #559: @100 = @2[4] */ - for (rr=(&w100), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w100), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #560: @279 = (@100*@99) */ - w279 = (w100*w99); + w279 = (w100 * w99); /* #561: (@1[12] += @279) */ - for (rr=w1+12, ss=(&w279); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w279); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #562: @279 = @2[12] */ - for (rr=(&w279), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w279), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #563: @99 = (@279*@99) */ - w99 = (w279*w99); + w99 = (w279 * w99); /* #564: (@1[4] += @99) */ - for (rr=w1+4, ss=(&w99); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w99); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #565: @99 = @2[4] */ - for (rr=(&w99), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w99), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #566: @280 = (@99*@98) */ - w280 = (w99*w98); + w280 = (w99 * w98); /* #567: (@1[11] += @280) */ - for (rr=w1+11, ss=(&w280); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w280); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #568: @280 = @2[11] */ - for (rr=(&w280), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w280), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #569: @98 = (@280*@98) */ - w98 = (w280*w98); + w98 = (w280 * w98); /* #570: (@1[4] += @98) */ - for (rr=w1+4, ss=(&w98); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w98); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #571: @98 = @2[4] */ - for (rr=(&w98), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w98), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #572: @281 = (@98*@97) */ - w281 = (w98*w97); + w281 = (w98 * w97); /* #573: (@1[10] += @281) */ - for (rr=w1+10, ss=(&w281); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w281); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #574: @281 = @2[10] */ - for (rr=(&w281), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w281), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #575: @97 = (@281*@97) */ - w97 = (w281*w97); + w97 = (w281 * w97); /* #576: (@1[4] += @97) */ - for (rr=w1+4, ss=(&w97); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w97); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #577: @97 = @2[4] */ - for (rr=(&w97), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w97), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #578: @282 = (@97*@96) */ - w282 = (w97*w96); + w282 = (w97 * w96); /* #579: (@1[9] += @282) */ - for (rr=w1+9, ss=(&w282); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w282); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #580: @282 = @2[9] */ - for (rr=(&w282), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w282), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #581: @96 = (@282*@96) */ - w96 = (w282*w96); + w96 = (w282 * w96); /* #582: (@1[4] += @96) */ - for (rr=w1+4, ss=(&w96); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w96); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #583: @96 = @2[4] */ - for (rr=(&w96), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w96), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #584: @283 = (@96*@95) */ - w283 = (w96*w95); + w283 = (w96 * w95); /* #585: (@1[8] += @283) */ - for (rr=w1+8, ss=(&w283); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w283); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #586: @283 = @2[8] */ - for (rr=(&w283), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w283), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #587: @95 = (@283*@95) */ - w95 = (w283*w95); + w95 = (w283 * w95); /* #588: (@1[4] += @95) */ - for (rr=w1+4, ss=(&w95); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w95); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #589: @95 = @2[4] */ - for (rr=(&w95), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w95), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #590: @284 = (@95*@94) */ - w284 = (w95*w94); + w284 = (w95 * w94); /* #591: (@1[7] += @284) */ - for (rr=w1+7, ss=(&w284); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w284); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #592: @284 = @2[7] */ - for (rr=(&w284), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w284), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #593: @94 = (@284*@94) */ - w94 = (w284*w94); + w94 = (w284 * w94); /* #594: (@1[4] += @94) */ - for (rr=w1+4, ss=(&w94); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w94); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #595: @94 = @2[4] */ - for (rr=(&w94), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w94), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #596: @285 = (@94*@93) */ - w285 = (w94*w93); + w285 = (w94 * w93); /* #597: (@1[6] += @285) */ - for (rr=w1+6, ss=(&w285); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w285); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #598: @285 = @2[6] */ - for (rr=(&w285), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w285), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #599: @93 = (@285*@93) */ - w93 = (w285*w93); + w93 = (w285 * w93); /* #600: (@1[4] += @93) */ - for (rr=w1+4, ss=(&w93); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w93); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #601: @93 = @2[4] */ - for (rr=(&w93), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w93), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #602: @286 = (@93*@92) */ - w286 = (w93*w92); + w286 = (w93 * w92); /* #603: (@1[5] += @286) */ - for (rr=w1+5, ss=(&w286); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w286); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #604: @286 = @2[5] */ - for (rr=(&w286), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w286), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #605: @92 = (@286*@92) */ - w92 = (w286*w92); + w92 = (w286 * w92); /* #606: (@1[4] += @92) */ - for (rr=w1+4, ss=(&w92); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w92); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #607: @92 = @2[4] */ - for (rr=(&w92), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w92), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #608: @92 = (2.*@92) */ - w92 = (2.* w92 ); + w92 = (2. * w92); /* #609: @91 = (@92*@91) */ - w91 = (w92*w91); + w91 = (w92 * w91); /* #610: (@1[4] += @91) */ - for (rr=w1+4, ss=(&w91); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w91); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #611: @91 = @2[3] */ - for (rr=(&w91), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w91), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #612: @287 = (@91*@90) */ - w287 = (w91*w90); + w287 = (w91 * w90); /* #613: (@1[17] += @287) */ - for (rr=w1+17, ss=(&w287); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w287); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #614: @287 = @2[17] */ - for (rr=(&w287), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w287), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #615: @90 = (@287*@90) */ - w90 = (w287*w90); + w90 = (w287 * w90); /* #616: (@1[3] += @90) */ - for (rr=w1+3, ss=(&w90); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w90); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #617: @90 = @2[3] */ - for (rr=(&w90), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w90), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #618: @288 = (@90*@89) */ - w288 = (w90*w89); + w288 = (w90 * w89); /* #619: (@1[16] += @288) */ - for (rr=w1+16, ss=(&w288); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w288); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #620: @288 = @2[16] */ - for (rr=(&w288), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w288), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #621: @89 = (@288*@89) */ - w89 = (w288*w89); + w89 = (w288 * w89); /* #622: (@1[3] += @89) */ - for (rr=w1+3, ss=(&w89); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w89); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #623: @89 = @2[3] */ - for (rr=(&w89), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w89), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #624: @289 = (@89*@88) */ - w289 = (w89*w88); + w289 = (w89 * w88); /* #625: (@1[15] += @289) */ - for (rr=w1+15, ss=(&w289); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w289); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #626: @289 = @2[15] */ - for (rr=(&w289), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w289), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #627: @88 = (@289*@88) */ - w88 = (w289*w88); + w88 = (w289 * w88); /* #628: (@1[3] += @88) */ - for (rr=w1+3, ss=(&w88); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w88); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #629: @88 = @2[3] */ - for (rr=(&w88), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w88), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #630: @290 = (@88*@87) */ - w290 = (w88*w87); + w290 = (w88 * w87); /* #631: (@1[14] += @290) */ - for (rr=w1+14, ss=(&w290); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w290); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #632: @290 = @2[14] */ - for (rr=(&w290), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w290), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #633: @87 = (@290*@87) */ - w87 = (w290*w87); + w87 = (w290 * w87); /* #634: (@1[3] += @87) */ - for (rr=w1+3, ss=(&w87); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w87); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #635: @87 = @2[3] */ - for (rr=(&w87), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w87), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #636: @291 = (@87*@86) */ - w291 = (w87*w86); + w291 = (w87 * w86); /* #637: (@1[13] += @291) */ - for (rr=w1+13, ss=(&w291); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w291); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #638: @291 = @2[13] */ - for (rr=(&w291), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w291), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #639: @86 = (@291*@86) */ - w86 = (w291*w86); + w86 = (w291 * w86); /* #640: (@1[3] += @86) */ - for (rr=w1+3, ss=(&w86); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w86); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #641: @86 = @2[3] */ - for (rr=(&w86), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w86), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #642: @292 = (@86*@85) */ - w292 = (w86*w85); + w292 = (w86 * w85); /* #643: (@1[12] += @292) */ - for (rr=w1+12, ss=(&w292); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w292); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #644: @292 = @2[12] */ - for (rr=(&w292), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w292), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #645: @85 = (@292*@85) */ - w85 = (w292*w85); + w85 = (w292 * w85); /* #646: (@1[3] += @85) */ - for (rr=w1+3, ss=(&w85); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w85); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #647: @85 = @2[3] */ - for (rr=(&w85), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w85), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #648: @293 = (@85*@84) */ - w293 = (w85*w84); + w293 = (w85 * w84); /* #649: (@1[11] += @293) */ - for (rr=w1+11, ss=(&w293); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w293); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #650: @293 = @2[11] */ - for (rr=(&w293), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w293), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #651: @84 = (@293*@84) */ - w84 = (w293*w84); + w84 = (w293 * w84); /* #652: (@1[3] += @84) */ - for (rr=w1+3, ss=(&w84); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w84); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #653: @84 = @2[3] */ - for (rr=(&w84), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w84), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #654: @294 = (@84*@83) */ - w294 = (w84*w83); + w294 = (w84 * w83); /* #655: (@1[10] += @294) */ - for (rr=w1+10, ss=(&w294); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w294); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #656: @294 = @2[10] */ - for (rr=(&w294), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w294), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #657: @83 = (@294*@83) */ - w83 = (w294*w83); + w83 = (w294 * w83); /* #658: (@1[3] += @83) */ - for (rr=w1+3, ss=(&w83); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w83); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #659: @83 = @2[3] */ - for (rr=(&w83), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w83), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #660: @295 = (@83*@82) */ - w295 = (w83*w82); + w295 = (w83 * w82); /* #661: (@1[9] += @295) */ - for (rr=w1+9, ss=(&w295); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w295); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #662: @295 = @2[9] */ - for (rr=(&w295), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w295), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #663: @82 = (@295*@82) */ - w82 = (w295*w82); + w82 = (w295 * w82); /* #664: (@1[3] += @82) */ - for (rr=w1+3, ss=(&w82); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w82); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #665: @82 = @2[3] */ - for (rr=(&w82), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w82), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #666: @296 = (@82*@81) */ - w296 = (w82*w81); + w296 = (w82 * w81); /* #667: (@1[8] += @296) */ - for (rr=w1+8, ss=(&w296); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w296); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #668: @296 = @2[8] */ - for (rr=(&w296), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w296), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #669: @81 = (@296*@81) */ - w81 = (w296*w81); + w81 = (w296 * w81); /* #670: (@1[3] += @81) */ - for (rr=w1+3, ss=(&w81); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w81); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #671: @81 = @2[3] */ - for (rr=(&w81), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w81), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #672: @297 = (@81*@80) */ - w297 = (w81*w80); + w297 = (w81 * w80); /* #673: (@1[7] += @297) */ - for (rr=w1+7, ss=(&w297); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w297); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #674: @297 = @2[7] */ - for (rr=(&w297), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w297), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #675: @80 = (@297*@80) */ - w80 = (w297*w80); + w80 = (w297 * w80); /* #676: (@1[3] += @80) */ - for (rr=w1+3, ss=(&w80); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w80); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #677: @80 = @2[3] */ - for (rr=(&w80), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w80), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #678: @298 = (@80*@79) */ - w298 = (w80*w79); + w298 = (w80 * w79); /* #679: (@1[6] += @298) */ - for (rr=w1+6, ss=(&w298); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w298); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #680: @298 = @2[6] */ - for (rr=(&w298), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w298), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #681: @79 = (@298*@79) */ - w79 = (w298*w79); + w79 = (w298 * w79); /* #682: (@1[3] += @79) */ - for (rr=w1+3, ss=(&w79); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w79); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #683: @79 = @2[3] */ - for (rr=(&w79), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w79), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #684: @299 = (@79*@78) */ - w299 = (w79*w78); + w299 = (w79 * w78); /* #685: (@1[5] += @299) */ - for (rr=w1+5, ss=(&w299); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w299); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #686: @299 = @2[5] */ - for (rr=(&w299), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w299), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #687: @78 = (@299*@78) */ - w78 = (w299*w78); + w78 = (w299 * w78); /* #688: (@1[3] += @78) */ - for (rr=w1+3, ss=(&w78); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w78); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #689: @78 = @2[3] */ - for (rr=(&w78), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w78), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #690: @300 = (@78*@77) */ - w300 = (w78*w77); + w300 = (w78 * w77); /* #691: (@1[4] += @300) */ - for (rr=w1+4, ss=(&w300); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w300); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #692: @300 = @2[4] */ - for (rr=(&w300), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w300), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #693: @77 = (@300*@77) */ - w77 = (w300*w77); + w77 = (w300 * w77); /* #694: (@1[3] += @77) */ - for (rr=w1+3, ss=(&w77); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w77); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #695: @77 = @2[3] */ - for (rr=(&w77), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w77), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #696: @77 = (2.*@77) */ - w77 = (2.* w77 ); + w77 = (2. * w77); /* #697: @76 = (@77*@76) */ - w76 = (w77*w76); + w76 = (w77 * w76); /* #698: (@1[3] += @76) */ - for (rr=w1+3, ss=(&w76); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w76); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #699: @76 = @2[2] */ - for (rr=(&w76), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w76), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #700: @301 = (@76*@75) */ - w301 = (w76*w75); + w301 = (w76 * w75); /* #701: (@1[17] += @301) */ - for (rr=w1+17, ss=(&w301); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w301); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #702: @301 = @2[17] */ - for (rr=(&w301), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w301), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #703: @75 = (@301*@75) */ - w75 = (w301*w75); + w75 = (w301 * w75); /* #704: (@1[2] += @75) */ - for (rr=w1+2, ss=(&w75); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w75); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #705: @75 = @2[2] */ - for (rr=(&w75), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w75), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #706: @302 = (@75*@74) */ - w302 = (w75*w74); + w302 = (w75 * w74); /* #707: (@1[16] += @302) */ - for (rr=w1+16, ss=(&w302); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w302); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #708: @302 = @2[16] */ - for (rr=(&w302), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w302), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #709: @74 = (@302*@74) */ - w74 = (w302*w74); + w74 = (w302 * w74); /* #710: (@1[2] += @74) */ - for (rr=w1+2, ss=(&w74); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w74); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #711: @74 = @2[2] */ - for (rr=(&w74), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w74), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #712: @303 = (@74*@73) */ - w303 = (w74*w73); + w303 = (w74 * w73); /* #713: (@1[15] += @303) */ - for (rr=w1+15, ss=(&w303); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w303); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #714: @303 = @2[15] */ - for (rr=(&w303), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w303), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #715: @73 = (@303*@73) */ - w73 = (w303*w73); + w73 = (w303 * w73); /* #716: (@1[2] += @73) */ - for (rr=w1+2, ss=(&w73); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w73); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #717: @73 = @2[2] */ - for (rr=(&w73), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w73), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #718: @304 = (@73*@72) */ - w304 = (w73*w72); + w304 = (w73 * w72); /* #719: (@1[14] += @304) */ - for (rr=w1+14, ss=(&w304); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w304); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #720: @304 = @2[14] */ - for (rr=(&w304), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w304), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #721: @72 = (@304*@72) */ - w72 = (w304*w72); + w72 = (w304 * w72); /* #722: (@1[2] += @72) */ - for (rr=w1+2, ss=(&w72); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w72); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #723: @72 = @2[2] */ - for (rr=(&w72), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w72), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #724: @305 = (@72*@71) */ - w305 = (w72*w71); + w305 = (w72 * w71); /* #725: (@1[13] += @305) */ - for (rr=w1+13, ss=(&w305); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w305); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #726: @305 = @2[13] */ - for (rr=(&w305), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w305), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #727: @71 = (@305*@71) */ - w71 = (w305*w71); + w71 = (w305 * w71); /* #728: (@1[2] += @71) */ - for (rr=w1+2, ss=(&w71); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w71); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #729: @71 = @2[2] */ - for (rr=(&w71), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w71), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #730: @306 = (@71*@70) */ - w306 = (w71*w70); + w306 = (w71 * w70); /* #731: (@1[12] += @306) */ - for (rr=w1+12, ss=(&w306); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w306); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #732: @306 = @2[12] */ - for (rr=(&w306), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w306), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #733: @70 = (@306*@70) */ - w70 = (w306*w70); + w70 = (w306 * w70); /* #734: (@1[2] += @70) */ - for (rr=w1+2, ss=(&w70); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w70); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #735: @70 = @2[2] */ - for (rr=(&w70), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w70), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #736: @307 = (@70*@69) */ - w307 = (w70*w69); + w307 = (w70 * w69); /* #737: (@1[11] += @307) */ - for (rr=w1+11, ss=(&w307); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w307); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #738: @307 = @2[11] */ - for (rr=(&w307), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w307), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #739: @69 = (@307*@69) */ - w69 = (w307*w69); + w69 = (w307 * w69); /* #740: (@1[2] += @69) */ - for (rr=w1+2, ss=(&w69); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w69); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #741: @69 = @2[2] */ - for (rr=(&w69), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w69), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #742: @308 = (@69*@68) */ - w308 = (w69*w68); + w308 = (w69 * w68); /* #743: (@1[10] += @308) */ - for (rr=w1+10, ss=(&w308); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w308); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #744: @308 = @2[10] */ - for (rr=(&w308), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w308), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #745: @68 = (@308*@68) */ - w68 = (w308*w68); + w68 = (w308 * w68); /* #746: (@1[2] += @68) */ - for (rr=w1+2, ss=(&w68); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w68); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #747: @68 = @2[2] */ - for (rr=(&w68), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w68), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #748: @309 = (@68*@67) */ - w309 = (w68*w67); + w309 = (w68 * w67); /* #749: (@1[9] += @309) */ - for (rr=w1+9, ss=(&w309); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w309); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #750: @309 = @2[9] */ - for (rr=(&w309), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w309), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #751: @67 = (@309*@67) */ - w67 = (w309*w67); + w67 = (w309 * w67); /* #752: (@1[2] += @67) */ - for (rr=w1+2, ss=(&w67); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w67); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #753: @67 = @2[2] */ - for (rr=(&w67), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w67), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #754: @310 = (@67*@66) */ - w310 = (w67*w66); + w310 = (w67 * w66); /* #755: (@1[8] += @310) */ - for (rr=w1+8, ss=(&w310); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w310); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #756: @310 = @2[8] */ - for (rr=(&w310), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w310), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #757: @66 = (@310*@66) */ - w66 = (w310*w66); + w66 = (w310 * w66); /* #758: (@1[2] += @66) */ - for (rr=w1+2, ss=(&w66); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w66); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #759: @66 = @2[2] */ - for (rr=(&w66), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w66), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #760: @311 = (@66*@65) */ - w311 = (w66*w65); + w311 = (w66 * w65); /* #761: (@1[7] += @311) */ - for (rr=w1+7, ss=(&w311); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w311); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #762: @311 = @2[7] */ - for (rr=(&w311), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w311), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #763: @65 = (@311*@65) */ - w65 = (w311*w65); + w65 = (w311 * w65); /* #764: (@1[2] += @65) */ - for (rr=w1+2, ss=(&w65); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w65); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #765: @65 = @2[2] */ - for (rr=(&w65), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w65), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #766: @312 = (@65*@64) */ - w312 = (w65*w64); + w312 = (w65 * w64); /* #767: (@1[6] += @312) */ - for (rr=w1+6, ss=(&w312); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w312); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #768: @312 = @2[6] */ - for (rr=(&w312), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w312), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #769: @64 = (@312*@64) */ - w64 = (w312*w64); + w64 = (w312 * w64); /* #770: (@1[2] += @64) */ - for (rr=w1+2, ss=(&w64); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w64); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #771: @64 = @2[2] */ - for (rr=(&w64), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w64), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #772: @313 = (@64*@63) */ - w313 = (w64*w63); + w313 = (w64 * w63); /* #773: (@1[5] += @313) */ - for (rr=w1+5, ss=(&w313); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w313); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #774: @313 = @2[5] */ - for (rr=(&w313), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w313), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #775: @63 = (@313*@63) */ - w63 = (w313*w63); + w63 = (w313 * w63); /* #776: (@1[2] += @63) */ - for (rr=w1+2, ss=(&w63); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w63); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #777: @63 = @2[2] */ - for (rr=(&w63), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w63), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #778: @314 = (@63*@62) */ - w314 = (w63*w62); + w314 = (w63 * w62); /* #779: (@1[4] += @314) */ - for (rr=w1+4, ss=(&w314); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w314); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #780: @314 = @2[4] */ - for (rr=(&w314), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w314), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #781: @62 = (@314*@62) */ - w62 = (w314*w62); + w62 = (w314 * w62); /* #782: (@1[2] += @62) */ - for (rr=w1+2, ss=(&w62); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w62); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #783: @62 = @2[2] */ - for (rr=(&w62), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w62), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #784: @315 = (@62*@61) */ - w315 = (w62*w61); + w315 = (w62 * w61); /* #785: (@1[3] += @315) */ - for (rr=w1+3, ss=(&w315); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w315); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #786: @315 = @2[3] */ - for (rr=(&w315), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w315), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #787: @61 = (@315*@61) */ - w61 = (w315*w61); + w61 = (w315 * w61); /* #788: (@1[2] += @61) */ - for (rr=w1+2, ss=(&w61); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w61); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #789: @61 = @2[2] */ - for (rr=(&w61), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w61), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #790: @61 = (2.*@61) */ - w61 = (2.* w61 ); + w61 = (2. * w61); /* #791: @60 = (@61*@60) */ - w60 = (w61*w60); + w60 = (w61 * w60); /* #792: (@1[2] += @60) */ - for (rr=w1+2, ss=(&w60); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w60); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #793: @60 = @2[1] */ - for (rr=(&w60), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w60), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #794: @316 = (@60*@59) */ - w316 = (w60*w59); + w316 = (w60 * w59); /* #795: (@1[17] += @316) */ - for (rr=w1+17, ss=(&w316); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w316); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #796: @316 = @2[17] */ - for (rr=(&w316), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w316), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #797: @59 = (@316*@59) */ - w59 = (w316*w59); + w59 = (w316 * w59); /* #798: (@1[1] += @59) */ - for (rr=w1+1, ss=(&w59); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w59); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #799: @59 = @2[1] */ - for (rr=(&w59), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w59), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #800: @317 = (@59*@58) */ - w317 = (w59*w58); + w317 = (w59 * w58); /* #801: (@1[16] += @317) */ - for (rr=w1+16, ss=(&w317); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w317); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #802: @317 = @2[16] */ - for (rr=(&w317), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w317), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #803: @58 = (@317*@58) */ - w58 = (w317*w58); + w58 = (w317 * w58); /* #804: (@1[1] += @58) */ - for (rr=w1+1, ss=(&w58); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w58); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #805: @58 = @2[1] */ - for (rr=(&w58), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w58), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #806: @318 = (@58*@57) */ - w318 = (w58*w57); + w318 = (w58 * w57); /* #807: (@1[15] += @318) */ - for (rr=w1+15, ss=(&w318); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w318); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #808: @318 = @2[15] */ - for (rr=(&w318), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w318), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #809: @57 = (@318*@57) */ - w57 = (w318*w57); + w57 = (w318 * w57); /* #810: (@1[1] += @57) */ - for (rr=w1+1, ss=(&w57); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w57); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #811: @57 = @2[1] */ - for (rr=(&w57), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w57), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #812: @319 = (@57*@56) */ - w319 = (w57*w56); + w319 = (w57 * w56); /* #813: (@1[14] += @319) */ - for (rr=w1+14, ss=(&w319); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w319); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #814: @319 = @2[14] */ - for (rr=(&w319), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w319), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #815: @56 = (@319*@56) */ - w56 = (w319*w56); + w56 = (w319 * w56); /* #816: (@1[1] += @56) */ - for (rr=w1+1, ss=(&w56); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w56); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #817: @56 = @2[1] */ - for (rr=(&w56), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w56), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #818: @320 = (@56*@55) */ - w320 = (w56*w55); + w320 = (w56 * w55); /* #819: (@1[13] += @320) */ - for (rr=w1+13, ss=(&w320); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w320); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #820: @320 = @2[13] */ - for (rr=(&w320), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w320), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #821: @55 = (@320*@55) */ - w55 = (w320*w55); + w55 = (w320 * w55); /* #822: (@1[1] += @55) */ - for (rr=w1+1, ss=(&w55); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w55); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #823: @55 = @2[1] */ - for (rr=(&w55), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w55), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #824: @321 = (@55*@54) */ - w321 = (w55*w54); + w321 = (w55 * w54); /* #825: (@1[12] += @321) */ - for (rr=w1+12, ss=(&w321); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w321); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #826: @321 = @2[12] */ - for (rr=(&w321), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w321), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #827: @54 = (@321*@54) */ - w54 = (w321*w54); + w54 = (w321 * w54); /* #828: (@1[1] += @54) */ - for (rr=w1+1, ss=(&w54); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w54); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #829: @54 = @2[1] */ - for (rr=(&w54), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w54), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #830: @322 = (@54*@53) */ - w322 = (w54*w53); + w322 = (w54 * w53); /* #831: (@1[11] += @322) */ - for (rr=w1+11, ss=(&w322); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w322); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #832: @322 = @2[11] */ - for (rr=(&w322), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w322), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #833: @53 = (@322*@53) */ - w53 = (w322*w53); + w53 = (w322 * w53); /* #834: (@1[1] += @53) */ - for (rr=w1+1, ss=(&w53); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w53); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #835: @53 = @2[1] */ - for (rr=(&w53), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w53), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #836: @323 = (@53*@52) */ - w323 = (w53*w52); + w323 = (w53 * w52); /* #837: (@1[10] += @323) */ - for (rr=w1+10, ss=(&w323); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w323); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #838: @323 = @2[10] */ - for (rr=(&w323), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w323), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #839: @52 = (@323*@52) */ - w52 = (w323*w52); + w52 = (w323 * w52); /* #840: (@1[1] += @52) */ - for (rr=w1+1, ss=(&w52); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w52); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #841: @52 = @2[1] */ - for (rr=(&w52), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w52), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #842: @324 = (@52*@51) */ - w324 = (w52*w51); + w324 = (w52 * w51); /* #843: (@1[9] += @324) */ - for (rr=w1+9, ss=(&w324); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w324); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #844: @324 = @2[9] */ - for (rr=(&w324), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w324), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #845: @51 = (@324*@51) */ - w51 = (w324*w51); + w51 = (w324 * w51); /* #846: (@1[1] += @51) */ - for (rr=w1+1, ss=(&w51); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w51); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #847: @51 = @2[1] */ - for (rr=(&w51), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w51), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #848: @325 = (@51*@50) */ - w325 = (w51*w50); + w325 = (w51 * w50); /* #849: (@1[8] += @325) */ - for (rr=w1+8, ss=(&w325); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w325); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #850: @325 = @2[8] */ - for (rr=(&w325), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w325), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #851: @50 = (@325*@50) */ - w50 = (w325*w50); + w50 = (w325 * w50); /* #852: (@1[1] += @50) */ - for (rr=w1+1, ss=(&w50); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w50); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #853: @50 = @2[1] */ - for (rr=(&w50), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w50), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #854: @326 = (@50*@49) */ - w326 = (w50*w49); + w326 = (w50 * w49); /* #855: (@1[7] += @326) */ - for (rr=w1+7, ss=(&w326); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w326); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #856: @326 = @2[7] */ - for (rr=(&w326), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w326), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #857: @49 = (@326*@49) */ - w49 = (w326*w49); + w49 = (w326 * w49); /* #858: (@1[1] += @49) */ - for (rr=w1+1, ss=(&w49); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w49); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #859: @49 = @2[1] */ - for (rr=(&w49), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w49), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #860: @327 = (@49*@48) */ - w327 = (w49*w48); + w327 = (w49 * w48); /* #861: (@1[6] += @327) */ - for (rr=w1+6, ss=(&w327); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w327); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #862: @327 = @2[6] */ - for (rr=(&w327), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w327), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #863: @48 = (@327*@48) */ - w48 = (w327*w48); + w48 = (w327 * w48); /* #864: (@1[1] += @48) */ - for (rr=w1+1, ss=(&w48); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w48); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #865: @48 = @2[1] */ - for (rr=(&w48), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w48), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #866: @328 = (@48*@47) */ - w328 = (w48*w47); + w328 = (w48 * w47); /* #867: (@1[5] += @328) */ - for (rr=w1+5, ss=(&w328); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w328); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #868: @328 = @2[5] */ - for (rr=(&w328), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w328), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #869: @47 = (@328*@47) */ - w47 = (w328*w47); + w47 = (w328 * w47); /* #870: (@1[1] += @47) */ - for (rr=w1+1, ss=(&w47); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w47); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #871: @47 = @2[1] */ - for (rr=(&w47), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w47), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #872: @329 = (@47*@46) */ - w329 = (w47*w46); + w329 = (w47 * w46); /* #873: (@1[4] += @329) */ - for (rr=w1+4, ss=(&w329); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w329); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #874: @329 = @2[4] */ - for (rr=(&w329), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w329), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #875: @46 = (@329*@46) */ - w46 = (w329*w46); + w46 = (w329 * w46); /* #876: (@1[1] += @46) */ - for (rr=w1+1, ss=(&w46); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w46); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #877: @46 = @2[1] */ - for (rr=(&w46), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w46), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #878: @330 = (@46*@45) */ - w330 = (w46*w45); + w330 = (w46 * w45); /* #879: (@1[3] += @330) */ - for (rr=w1+3, ss=(&w330); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w330); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #880: @330 = @2[3] */ - for (rr=(&w330), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w330), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #881: @45 = (@330*@45) */ - w45 = (w330*w45); + w45 = (w330 * w45); /* #882: (@1[1] += @45) */ - for (rr=w1+1, ss=(&w45); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w45); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #883: @45 = @2[1] */ - for (rr=(&w45), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w45), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #884: @331 = (@45*@44) */ - w331 = (w45*w44); + w331 = (w45 * w44); /* #885: (@1[2] += @331) */ - for (rr=w1+2, ss=(&w331); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w331); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #886: @331 = @2[2] */ - for (rr=(&w331), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w331), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #887: @44 = (@331*@44) */ - w44 = (w331*w44); + w44 = (w331 * w44); /* #888: (@1[1] += @44) */ - for (rr=w1+1, ss=(&w44); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w44); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #889: @44 = @2[1] */ - for (rr=(&w44), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w44), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #890: @44 = (2.*@44) */ - w44 = (2.* w44 ); + w44 = (2. * w44); /* #891: @43 = (@44*@43) */ - w43 = (w44*w43); + w43 = (w44 * w43); /* #892: (@1[1] += @43) */ - for (rr=w1+1, ss=(&w43); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w43); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #893: @43 = @2[0] */ - for (rr=(&w43), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w43), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #894: @332 = (@43*@42) */ - w332 = (w43*w42); + w332 = (w43 * w42); /* #895: (@1[17] += @332) */ - for (rr=w1+17, ss=(&w332); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w332); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #896: @332 = @2[17] */ - for (rr=(&w332), ss=w2+17; ss!=w2+18; ss+=1) *rr++ = *ss; + for (rr = (&w332), ss = w2 + 17; ss != w2 + 18; ss += 1) + *rr++ = *ss; /* #897: @42 = (@332*@42) */ - w42 = (w332*w42); + w42 = (w332 * w42); /* #898: (@1[0] += @42) */ - for (rr=w1+0, ss=(&w42); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w42); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #899: @42 = @2[0] */ - for (rr=(&w42), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w42), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #900: @333 = (@42*@41) */ - w333 = (w42*w41); + w333 = (w42 * w41); /* #901: (@1[16] += @333) */ - for (rr=w1+16, ss=(&w333); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w333); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #902: @333 = @2[16] */ - for (rr=(&w333), ss=w2+16; ss!=w2+17; ss+=1) *rr++ = *ss; + for (rr = (&w333), ss = w2 + 16; ss != w2 + 17; ss += 1) + *rr++ = *ss; /* #903: @41 = (@333*@41) */ - w41 = (w333*w41); + w41 = (w333 * w41); /* #904: (@1[0] += @41) */ - for (rr=w1+0, ss=(&w41); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w41); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #905: @41 = @2[0] */ - for (rr=(&w41), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w41), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #906: @334 = (@41*@40) */ - w334 = (w41*w40); + w334 = (w41 * w40); /* #907: (@1[15] += @334) */ - for (rr=w1+15, ss=(&w334); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w334); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #908: @334 = @2[15] */ - for (rr=(&w334), ss=w2+15; ss!=w2+16; ss+=1) *rr++ = *ss; + for (rr = (&w334), ss = w2 + 15; ss != w2 + 16; ss += 1) + *rr++ = *ss; /* #909: @40 = (@334*@40) */ - w40 = (w334*w40); + w40 = (w334 * w40); /* #910: (@1[0] += @40) */ - for (rr=w1+0, ss=(&w40); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w40); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #911: @40 = @2[0] */ - for (rr=(&w40), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w40), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #912: @335 = (@40*@39) */ - w335 = (w40*w39); + w335 = (w40 * w39); /* #913: (@1[14] += @335) */ - for (rr=w1+14, ss=(&w335); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w335); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #914: @335 = @2[14] */ - for (rr=(&w335), ss=w2+14; ss!=w2+15; ss+=1) *rr++ = *ss; + for (rr = (&w335), ss = w2 + 14; ss != w2 + 15; ss += 1) + *rr++ = *ss; /* #915: @39 = (@335*@39) */ - w39 = (w335*w39); + w39 = (w335 * w39); /* #916: (@1[0] += @39) */ - for (rr=w1+0, ss=(&w39); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w39); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #917: @39 = @2[0] */ - for (rr=(&w39), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w39), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #918: @336 = (@39*@38) */ - w336 = (w39*w38); + w336 = (w39 * w38); /* #919: (@1[13] += @336) */ - for (rr=w1+13, ss=(&w336); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w336); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #920: @336 = @2[13] */ - for (rr=(&w336), ss=w2+13; ss!=w2+14; ss+=1) *rr++ = *ss; + for (rr = (&w336), ss = w2 + 13; ss != w2 + 14; ss += 1) + *rr++ = *ss; /* #921: @38 = (@336*@38) */ - w38 = (w336*w38); + w38 = (w336 * w38); /* #922: (@1[0] += @38) */ - for (rr=w1+0, ss=(&w38); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w38); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #923: @38 = @2[0] */ - for (rr=(&w38), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w38), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #924: @337 = (@38*@37) */ - w337 = (w38*w37); + w337 = (w38 * w37); /* #925: (@1[12] += @337) */ - for (rr=w1+12, ss=(&w337); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w337); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #926: @337 = @2[12] */ - for (rr=(&w337), ss=w2+12; ss!=w2+13; ss+=1) *rr++ = *ss; + for (rr = (&w337), ss = w2 + 12; ss != w2 + 13; ss += 1) + *rr++ = *ss; /* #927: @37 = (@337*@37) */ - w37 = (w337*w37); + w37 = (w337 * w37); /* #928: (@1[0] += @37) */ - for (rr=w1+0, ss=(&w37); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w37); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #929: @37 = @2[0] */ - for (rr=(&w37), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w37), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #930: @338 = (@37*@36) */ - w338 = (w37*w36); + w338 = (w37 * w36); /* #931: (@1[11] += @338) */ - for (rr=w1+11, ss=(&w338); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w338); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #932: @338 = @2[11] */ - for (rr=(&w338), ss=w2+11; ss!=w2+12; ss+=1) *rr++ = *ss; + for (rr = (&w338), ss = w2 + 11; ss != w2 + 12; ss += 1) + *rr++ = *ss; /* #933: @36 = (@338*@36) */ - w36 = (w338*w36); + w36 = (w338 * w36); /* #934: (@1[0] += @36) */ - for (rr=w1+0, ss=(&w36); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w36); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #935: @36 = @2[0] */ - for (rr=(&w36), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w36), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #936: @339 = (@36*@35) */ - w339 = (w36*w35); + w339 = (w36 * w35); /* #937: (@1[10] += @339) */ - for (rr=w1+10, ss=(&w339); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w339); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #938: @339 = @2[10] */ - for (rr=(&w339), ss=w2+10; ss!=w2+11; ss+=1) *rr++ = *ss; + for (rr = (&w339), ss = w2 + 10; ss != w2 + 11; ss += 1) + *rr++ = *ss; /* #939: @35 = (@339*@35) */ - w35 = (w339*w35); + w35 = (w339 * w35); /* #940: (@1[0] += @35) */ - for (rr=w1+0, ss=(&w35); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w35); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #941: @35 = @2[0] */ - for (rr=(&w35), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w35), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #942: @340 = (@35*@34) */ - w340 = (w35*w34); + w340 = (w35 * w34); /* #943: (@1[9] += @340) */ - for (rr=w1+9, ss=(&w340); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w340); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #944: @340 = @2[9] */ - for (rr=(&w340), ss=w2+9; ss!=w2+10; ss+=1) *rr++ = *ss; + for (rr = (&w340), ss = w2 + 9; ss != w2 + 10; ss += 1) + *rr++ = *ss; /* #945: @34 = (@340*@34) */ - w34 = (w340*w34); + w34 = (w340 * w34); /* #946: (@1[0] += @34) */ - for (rr=w1+0, ss=(&w34); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w34); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #947: @34 = @2[0] */ - for (rr=(&w34), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w34), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #948: @341 = (@34*@33) */ - w341 = (w34*w33); + w341 = (w34 * w33); /* #949: (@1[8] += @341) */ - for (rr=w1+8, ss=(&w341); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w341); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #950: @341 = @2[8] */ - for (rr=(&w341), ss=w2+8; ss!=w2+9; ss+=1) *rr++ = *ss; + for (rr = (&w341), ss = w2 + 8; ss != w2 + 9; ss += 1) + *rr++ = *ss; /* #951: @33 = (@341*@33) */ - w33 = (w341*w33); + w33 = (w341 * w33); /* #952: (@1[0] += @33) */ - for (rr=w1+0, ss=(&w33); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w33); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #953: @33 = @2[0] */ - for (rr=(&w33), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w33), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #954: @342 = (@33*@32) */ - w342 = (w33*w32); + w342 = (w33 * w32); /* #955: (@1[7] += @342) */ - for (rr=w1+7, ss=(&w342); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w342); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #956: @342 = @2[7] */ - for (rr=(&w342), ss=w2+7; ss!=w2+8; ss+=1) *rr++ = *ss; + for (rr = (&w342), ss = w2 + 7; ss != w2 + 8; ss += 1) + *rr++ = *ss; /* #957: @32 = (@342*@32) */ - w32 = (w342*w32); + w32 = (w342 * w32); /* #958: (@1[0] += @32) */ - for (rr=w1+0, ss=(&w32); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w32); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #959: @32 = @2[0] */ - for (rr=(&w32), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w32), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #960: @343 = (@32*@31) */ - w343 = (w32*w31); + w343 = (w32 * w31); /* #961: (@1[6] += @343) */ - for (rr=w1+6, ss=(&w343); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w343); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #962: @343 = @2[6] */ - for (rr=(&w343), ss=w2+6; ss!=w2+7; ss+=1) *rr++ = *ss; + for (rr = (&w343), ss = w2 + 6; ss != w2 + 7; ss += 1) + *rr++ = *ss; /* #963: @31 = (@343*@31) */ - w31 = (w343*w31); + w31 = (w343 * w31); /* #964: (@1[0] += @31) */ - for (rr=w1+0, ss=(&w31); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w31); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #965: @31 = @2[0] */ - for (rr=(&w31), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w31), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #966: @344 = (@31*@30) */ - w344 = (w31*w30); + w344 = (w31 * w30); /* #967: (@1[5] += @344) */ - for (rr=w1+5, ss=(&w344); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w344); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #968: @344 = @2[5] */ - for (rr=(&w344), ss=w2+5; ss!=w2+6; ss+=1) *rr++ = *ss; + for (rr = (&w344), ss = w2 + 5; ss != w2 + 6; ss += 1) + *rr++ = *ss; /* #969: @30 = (@344*@30) */ - w30 = (w344*w30); + w30 = (w344 * w30); /* #970: (@1[0] += @30) */ - for (rr=w1+0, ss=(&w30); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w30); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #971: @30 = @2[0] */ - for (rr=(&w30), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w30), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #972: @345 = (@30*@29) */ - w345 = (w30*w29); + w345 = (w30 * w29); /* #973: (@1[4] += @345) */ - for (rr=w1+4, ss=(&w345); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w345); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #974: @345 = @2[4] */ - for (rr=(&w345), ss=w2+4; ss!=w2+5; ss+=1) *rr++ = *ss; + for (rr = (&w345), ss = w2 + 4; ss != w2 + 5; ss += 1) + *rr++ = *ss; /* #975: @29 = (@345*@29) */ - w29 = (w345*w29); + w29 = (w345 * w29); /* #976: (@1[0] += @29) */ - for (rr=w1+0, ss=(&w29); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w29); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #977: @29 = @2[0] */ - for (rr=(&w29), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w29), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #978: @346 = (@29*@28) */ - w346 = (w29*w28); + w346 = (w29 * w28); /* #979: (@1[3] += @346) */ - for (rr=w1+3, ss=(&w346); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w346); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #980: @346 = @2[3] */ - for (rr=(&w346), ss=w2+3; ss!=w2+4; ss+=1) *rr++ = *ss; + for (rr = (&w346), ss = w2 + 3; ss != w2 + 4; ss += 1) + *rr++ = *ss; /* #981: @28 = (@346*@28) */ - w28 = (w346*w28); + w28 = (w346 * w28); /* #982: (@1[0] += @28) */ - for (rr=w1+0, ss=(&w28); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w28); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #983: @28 = @2[0] */ - for (rr=(&w28), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w28), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #984: @347 = (@28*@27) */ - w347 = (w28*w27); + w347 = (w28 * w27); /* #985: (@1[2] += @347) */ - for (rr=w1+2, ss=(&w347); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w347); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #986: @347 = @2[2] */ - for (rr=(&w347), ss=w2+2; ss!=w2+3; ss+=1) *rr++ = *ss; + for (rr = (&w347), ss = w2 + 2; ss != w2 + 3; ss += 1) + *rr++ = *ss; /* #987: @27 = (@347*@27) */ - w27 = (w347*w27); + w27 = (w347 * w27); /* #988: (@1[0] += @27) */ - for (rr=w1+0, ss=(&w27); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w27); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #989: @27 = @2[0] */ - for (rr=(&w27), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w27), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #990: @348 = (@27*@26) */ - w348 = (w27*w26); + w348 = (w27 * w26); /* #991: (@1[1] += @348) */ - for (rr=w1+1, ss=(&w348); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w348); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #992: @348 = @2[1] */ - for (rr=(&w348), ss=w2+1; ss!=w2+2; ss+=1) *rr++ = *ss; + for (rr = (&w348), ss = w2 + 1; ss != w2 + 2; ss += 1) + *rr++ = *ss; /* #993: @26 = (@348*@26) */ - w26 = (w348*w26); + w26 = (w348 * w26); /* #994: (@1[0] += @26) */ - for (rr=w1+0, ss=(&w26); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w26); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #995: @26 = @2[0] */ - for (rr=(&w26), ss=w2+0; ss!=w2+1; ss+=1) *rr++ = *ss; + for (rr = (&w26), ss = w2 + 0; ss != w2 + 1; ss += 1) + *rr++ = *ss; /* #996: @26 = (2.*@26) */ - w26 = (2.* w26 ); + w26 = (2. * w26); /* #997: @25 = (@26*@25) */ - w25 = (w26*w25); + w25 = (w26 * w25); /* #998: (@1[0] += @25) */ - for (rr=w1+0, ss=(&w25); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w25); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #999: (@1[17] += @24) */ - for (rr=w1+17, ss=(&w24); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w24); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1000: (@1[16] += @23) */ - for (rr=w1+16, ss=(&w23); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w23); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1001: (@1[15] += @22) */ - for (rr=w1+15, ss=(&w22); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w22); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1002: (@1[14] += @21) */ - for (rr=w1+14, ss=(&w21); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w21); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1003: (@1[13] += @20) */ - for (rr=w1+13, ss=(&w20); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w20); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1004: (@1[12] += @19) */ - for (rr=w1+12, ss=(&w19); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w19); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1005: (@1[11] += @18) */ - for (rr=w1+11, ss=(&w18); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w18); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1006: (@1[10] += @17) */ - for (rr=w1+10, ss=(&w17); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w17); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1007: (@1[9] += @16) */ - for (rr=w1+9, ss=(&w16); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w16); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1008: (@1[8] += @15) */ - for (rr=w1+8, ss=(&w15); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w15); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1009: (@1[7] += @14) */ - for (rr=w1+7, ss=(&w14); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w14); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1010: (@1[6] += @13) */ - for (rr=w1+6, ss=(&w13); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w13); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1011: (@1[5] += @12) */ - for (rr=w1+5, ss=(&w12); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w12); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1012: (@1[4] += @11) */ - for (rr=w1+4, ss=(&w11); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w11); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1013: (@1[3] += @10) */ - for (rr=w1+3, ss=(&w10); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w10); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1014: (@1[2] += @9) */ - for (rr=w1+2, ss=(&w9); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w9); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1015: (@1[1] += @8) */ - for (rr=w1+1, ss=(&w8); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w8); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1016: (@1[0] += @7) */ - for (rr=w1+0, ss=(&w7); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w7); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1017: @7 = @1[0] */ - for (rr=(&w7), ss=w1+0; ss!=w1+1; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 0; ss != w1 + 1; ss += 1) + *rr++ = *ss; /* #1018: (@0[0] = @7) */ - for (rr=w0+0, ss=(&w7); rr!=w0+1; rr+=1) *rr = *ss++; + for (rr = w0 + 0, ss = (&w7); rr != w0 + 1; rr += 1) + *rr = *ss++; /* #1019: @7 = @1[1] */ - for (rr=(&w7), ss=w1+1; ss!=w1+2; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 1; ss != w1 + 2; ss += 1) + *rr++ = *ss; /* #1020: (@0[1] = @7) */ - for (rr=w0+1, ss=(&w7); rr!=w0+2; rr+=1) *rr = *ss++; + for (rr = w0 + 1, ss = (&w7); rr != w0 + 2; rr += 1) + *rr = *ss++; /* #1021: @7 = @1[2] */ - for (rr=(&w7), ss=w1+2; ss!=w1+3; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 2; ss != w1 + 3; ss += 1) + *rr++ = *ss; /* #1022: (@0[2] = @7) */ - for (rr=w0+2, ss=(&w7); rr!=w0+3; rr+=1) *rr = *ss++; + for (rr = w0 + 2, ss = (&w7); rr != w0 + 3; rr += 1) + *rr = *ss++; /* #1023: @7 = @1[3] */ - for (rr=(&w7), ss=w1+3; ss!=w1+4; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 3; ss != w1 + 4; ss += 1) + *rr++ = *ss; /* #1024: (@0[3] = @7) */ - for (rr=w0+3, ss=(&w7); rr!=w0+4; rr+=1) *rr = *ss++; + for (rr = w0 + 3, ss = (&w7); rr != w0 + 4; rr += 1) + *rr = *ss++; /* #1025: @7 = @1[4] */ - for (rr=(&w7), ss=w1+4; ss!=w1+5; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 4; ss != w1 + 5; ss += 1) + *rr++ = *ss; /* #1026: (@0[4] = @7) */ - for (rr=w0+4, ss=(&w7); rr!=w0+5; rr+=1) *rr = *ss++; + for (rr = w0 + 4, ss = (&w7); rr != w0 + 5; rr += 1) + *rr = *ss++; /* #1027: @7 = @1[5] */ - for (rr=(&w7), ss=w1+5; ss!=w1+6; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 5; ss != w1 + 6; ss += 1) + *rr++ = *ss; /* #1028: (@0[5] = @7) */ - for (rr=w0+5, ss=(&w7); rr!=w0+6; rr+=1) *rr = *ss++; + for (rr = w0 + 5, ss = (&w7); rr != w0 + 6; rr += 1) + *rr = *ss++; /* #1029: @7 = @1[6] */ - for (rr=(&w7), ss=w1+6; ss!=w1+7; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 6; ss != w1 + 7; ss += 1) + *rr++ = *ss; /* #1030: (@0[6] = @7) */ - for (rr=w0+6, ss=(&w7); rr!=w0+7; rr+=1) *rr = *ss++; + for (rr = w0 + 6, ss = (&w7); rr != w0 + 7; rr += 1) + *rr = *ss++; /* #1031: @7 = @1[7] */ - for (rr=(&w7), ss=w1+7; ss!=w1+8; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 7; ss != w1 + 8; ss += 1) + *rr++ = *ss; /* #1032: (@0[7] = @7) */ - for (rr=w0+7, ss=(&w7); rr!=w0+8; rr+=1) *rr = *ss++; + for (rr = w0 + 7, ss = (&w7); rr != w0 + 8; rr += 1) + *rr = *ss++; /* #1033: @7 = @1[8] */ - for (rr=(&w7), ss=w1+8; ss!=w1+9; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 8; ss != w1 + 9; ss += 1) + *rr++ = *ss; /* #1034: (@0[8] = @7) */ - for (rr=w0+8, ss=(&w7); rr!=w0+9; rr+=1) *rr = *ss++; + for (rr = w0 + 8, ss = (&w7); rr != w0 + 9; rr += 1) + *rr = *ss++; /* #1035: @7 = @1[9] */ - for (rr=(&w7), ss=w1+9; ss!=w1+10; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 9; ss != w1 + 10; ss += 1) + *rr++ = *ss; /* #1036: (@0[9] = @7) */ - for (rr=w0+9, ss=(&w7); rr!=w0+10; rr+=1) *rr = *ss++; + for (rr = w0 + 9, ss = (&w7); rr != w0 + 10; rr += 1) + *rr = *ss++; /* #1037: @7 = @1[10] */ - for (rr=(&w7), ss=w1+10; ss!=w1+11; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 10; ss != w1 + 11; ss += 1) + *rr++ = *ss; /* #1038: (@0[10] = @7) */ - for (rr=w0+10, ss=(&w7); rr!=w0+11; rr+=1) *rr = *ss++; + for (rr = w0 + 10, ss = (&w7); rr != w0 + 11; rr += 1) + *rr = *ss++; /* #1039: @7 = @1[11] */ - for (rr=(&w7), ss=w1+11; ss!=w1+12; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 11; ss != w1 + 12; ss += 1) + *rr++ = *ss; /* #1040: (@0[11] = @7) */ - for (rr=w0+11, ss=(&w7); rr!=w0+12; rr+=1) *rr = *ss++; + for (rr = w0 + 11, ss = (&w7); rr != w0 + 12; rr += 1) + *rr = *ss++; /* #1041: @7 = @1[12] */ - for (rr=(&w7), ss=w1+12; ss!=w1+13; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 12; ss != w1 + 13; ss += 1) + *rr++ = *ss; /* #1042: (@0[12] = @7) */ - for (rr=w0+12, ss=(&w7); rr!=w0+13; rr+=1) *rr = *ss++; + for (rr = w0 + 12, ss = (&w7); rr != w0 + 13; rr += 1) + *rr = *ss++; /* #1043: @7 = @1[13] */ - for (rr=(&w7), ss=w1+13; ss!=w1+14; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 13; ss != w1 + 14; ss += 1) + *rr++ = *ss; /* #1044: (@0[13] = @7) */ - for (rr=w0+13, ss=(&w7); rr!=w0+14; rr+=1) *rr = *ss++; + for (rr = w0 + 13, ss = (&w7); rr != w0 + 14; rr += 1) + *rr = *ss++; /* #1045: @7 = @1[14] */ - for (rr=(&w7), ss=w1+14; ss!=w1+15; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 14; ss != w1 + 15; ss += 1) + *rr++ = *ss; /* #1046: (@0[14] = @7) */ - for (rr=w0+14, ss=(&w7); rr!=w0+15; rr+=1) *rr = *ss++; + for (rr = w0 + 14, ss = (&w7); rr != w0 + 15; rr += 1) + *rr = *ss++; /* #1047: @7 = @1[15] */ - for (rr=(&w7), ss=w1+15; ss!=w1+16; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 15; ss != w1 + 16; ss += 1) + *rr++ = *ss; /* #1048: (@0[15] = @7) */ - for (rr=w0+15, ss=(&w7); rr!=w0+16; rr+=1) *rr = *ss++; + for (rr = w0 + 15, ss = (&w7); rr != w0 + 16; rr += 1) + *rr = *ss++; /* #1049: @7 = @1[16] */ - for (rr=(&w7), ss=w1+16; ss!=w1+17; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 16; ss != w1 + 17; ss += 1) + *rr++ = *ss; /* #1050: (@0[16] = @7) */ - for (rr=w0+16, ss=(&w7); rr!=w0+17; rr+=1) *rr = *ss++; + for (rr = w0 + 16, ss = (&w7); rr != w0 + 17; rr += 1) + *rr = *ss++; /* #1051: @7 = @1[17] */ - for (rr=(&w7), ss=w1+17; ss!=w1+18; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 17; ss != w1 + 18; ss += 1) + *rr++ = *ss; /* #1052: (@0[17] = @7) */ - for (rr=w0+17, ss=(&w7); rr!=w0+18; rr+=1) *rr = *ss++; + for (rr = w0 + 17, ss = (&w7); rr != w0 + 18; rr += 1) + *rr = *ss++; /* #1053: @1 = zeros(18x1) */ casadi_clear(w1, 18); /* #1054: @4 = zeros(189x1) */ casadi_clear(w4, 189); /* #1055: @6 = @5' */ - for (i=0, rr=w6, cs=w5; i<189; ++i) for (j=0; j<3; ++j) rr[i+j*189] = *cs++; + for (i = 0, rr = w6, cs = w5; i < 189; ++i) + for (j = 0; j < 3; ++j) + rr[i + j * 189] = *cs++; /* #1056: @7 = ones(3x1,1nz) */ w7 = 1.; /* #1057: @4 = mac(@6,@7,@4) */ casadi_mtimes(w6, casadi_s2, (&w7), casadi_s5, w4, casadi_s4, w, 0); - /* #1058: {@7, @8, @9, @10, @11, @12, @13, @14, @15, @16, @17, @18, @19, @20, @21, @22, @23, @24, @25, @349, @350, @351, @352, @353, @354, @355, @356, @357, @358, @359, @360, @361, @362, @363, @364, @365, @366, @367, @368, @369, @370, @371, @372, @373, @374, @375, @376, @377, @378, @379, @380, @381, @382, @383, @384, @385, @386, @387, @388, @389, @390, @391, @392, @393, @394, @395, @396, @397, @398, @399, @400, @401, @402, @403, @404, @405, @406, @407, @408, @409, @410, @411, @412, @413, @414, @415, @416, @417, @418, @419, @420, @421, @422, @423, @424, @425, @426, @427, @428, @429, @430, @431, @432, @433, @434, @435, @436, @437, @438, @439, @440, @441, @442, @443, @444, @445, @446, @447, @448, @449, @450, @451, @452, @453, @454, @455, @456, @457, @458, @459, @460, @461, @462, @463, @464, @465, @466, @467, @468, @469, @470, @471, @472, @473, @474, @475, @476, @477, @478, @479, @480, @481, @482, @483, @484, @485, @486, @487, @488, @489, @490, @491, @492, @493, @494, @495, @496, @497, @498, @499, @500, @501, @502, @503, @504, @505, @506, @507, @508, @509, @510, @511, @512, @513, @514, @515, @516, @517, @518} = vertsplit(@4) */ + /* #1058: {@7, @8, @9, @10, @11, @12, @13, @14, @15, @16, @17, @18, @19, @20, + * @21, @22, @23, @24, @25, @349, @350, @351, @352, @353, @354, @355, @356, + * @357, @358, @359, @360, @361, @362, @363, @364, @365, @366, @367, @368, + * @369, @370, @371, @372, @373, @374, @375, @376, @377, @378, @379, @380, + * @381, @382, @383, @384, @385, @386, @387, @388, @389, @390, @391, @392, + * @393, @394, @395, @396, @397, @398, @399, @400, @401, @402, @403, @404, + * @405, @406, @407, @408, @409, @410, @411, @412, @413, @414, @415, @416, + * @417, @418, @419, @420, @421, @422, @423, @424, @425, @426, @427, @428, + * @429, @430, @431, @432, @433, @434, @435, @436, @437, @438, @439, @440, + * @441, @442, @443, @444, @445, @446, @447, @448, @449, @450, @451, @452, + * @453, @454, @455, @456, @457, @458, @459, @460, @461, @462, @463, @464, + * @465, @466, @467, @468, @469, @470, @471, @472, @473, @474, @475, @476, + * @477, @478, @479, @480, @481, @482, @483, @484, @485, @486, @487, @488, + * @489, @490, @491, @492, @493, @494, @495, @496, @497, @498, @499, @500, + * @501, @502, @503, @504, @505, @506, @507, @508, @509, @510, @511, @512, + * @513, @514, @515, @516, @517, @518} = vertsplit(@4) */ w7 = w4[0]; w8 = w4[1]; w9 = w4[2]; @@ -3948,1420 +5554,1815 @@ static int casadi_f1(const casadi_real** arg, casadi_real** res, casadi_int* iw, w517 = w4[187]; w518 = w4[188]; /* #1059: @518 = (@3*@518) */ - w518 = (w3*w518); + w518 = (w3 * w518); /* #1060: (@1[17] += @518) */ - for (rr=w1+17, ss=(&w518); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w518); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1061: @518 = (@195*@517) */ - w518 = (w195*w517); + w518 = (w195 * w517); /* #1062: (@1[17] += @518) */ - for (rr=w1+17, ss=(&w518); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w518); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1063: @517 = (@196*@517) */ - w517 = (w196*w517); + w517 = (w196 * w517); /* #1064: (@1[16] += @517) */ - for (rr=w1+16, ss=(&w517); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w517); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1065: @516 = (@194*@516) */ - w516 = (w194*w516); + w516 = (w194 * w516); /* #1066: (@1[16] += @516) */ - for (rr=w1+16, ss=(&w516); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w516); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1067: @516 = (@193*@515) */ - w516 = (w193*w515); + w516 = (w193 * w515); /* #1068: (@1[17] += @516) */ - for (rr=w1+17, ss=(&w516); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w516); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1069: @515 = (@197*@515) */ - w515 = (w197*w515); + w515 = (w197 * w515); /* #1070: (@1[15] += @515) */ - for (rr=w1+15, ss=(&w515); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w515); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1071: @515 = (@192*@514) */ - w515 = (w192*w514); + w515 = (w192 * w514); /* #1072: (@1[16] += @515) */ - for (rr=w1+16, ss=(&w515); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w515); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1073: @514 = (@198*@514) */ - w514 = (w198*w514); + w514 = (w198 * w514); /* #1074: (@1[15] += @514) */ - for (rr=w1+15, ss=(&w514); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w514); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1075: @513 = (@191*@513) */ - w513 = (w191*w513); + w513 = (w191 * w513); /* #1076: (@1[15] += @513) */ - for (rr=w1+15, ss=(&w513); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w513); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1077: @513 = (@190*@512) */ - w513 = (w190*w512); + w513 = (w190 * w512); /* #1078: (@1[17] += @513) */ - for (rr=w1+17, ss=(&w513); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w513); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1079: @512 = (@199*@512) */ - w512 = (w199*w512); + w512 = (w199 * w512); /* #1080: (@1[14] += @512) */ - for (rr=w1+14, ss=(&w512); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w512); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1081: @512 = (@189*@511) */ - w512 = (w189*w511); + w512 = (w189 * w511); /* #1082: (@1[16] += @512) */ - for (rr=w1+16, ss=(&w512); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w512); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1083: @511 = (@200*@511) */ - w511 = (w200*w511); + w511 = (w200 * w511); /* #1084: (@1[14] += @511) */ - for (rr=w1+14, ss=(&w511); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w511); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1085: @511 = (@188*@510) */ - w511 = (w188*w510); + w511 = (w188 * w510); /* #1086: (@1[15] += @511) */ - for (rr=w1+15, ss=(&w511); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w511); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1087: @510 = (@201*@510) */ - w510 = (w201*w510); + w510 = (w201 * w510); /* #1088: (@1[14] += @510) */ - for (rr=w1+14, ss=(&w510); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w510); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1089: @509 = (@187*@509) */ - w509 = (w187*w509); + w509 = (w187 * w509); /* #1090: (@1[14] += @509) */ - for (rr=w1+14, ss=(&w509); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w509); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1091: @509 = (@186*@508) */ - w509 = (w186*w508); + w509 = (w186 * w508); /* #1092: (@1[17] += @509) */ - for (rr=w1+17, ss=(&w509); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w509); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1093: @508 = (@202*@508) */ - w508 = (w202*w508); + w508 = (w202 * w508); /* #1094: (@1[13] += @508) */ - for (rr=w1+13, ss=(&w508); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w508); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1095: @508 = (@185*@507) */ - w508 = (w185*w507); + w508 = (w185 * w507); /* #1096: (@1[16] += @508) */ - for (rr=w1+16, ss=(&w508); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w508); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1097: @507 = (@203*@507) */ - w507 = (w203*w507); + w507 = (w203 * w507); /* #1098: (@1[13] += @507) */ - for (rr=w1+13, ss=(&w507); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w507); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1099: @507 = (@184*@506) */ - w507 = (w184*w506); + w507 = (w184 * w506); /* #1100: (@1[15] += @507) */ - for (rr=w1+15, ss=(&w507); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w507); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1101: @506 = (@204*@506) */ - w506 = (w204*w506); + w506 = (w204 * w506); /* #1102: (@1[13] += @506) */ - for (rr=w1+13, ss=(&w506); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w506); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1103: @506 = (@183*@505) */ - w506 = (w183*w505); + w506 = (w183 * w505); /* #1104: (@1[14] += @506) */ - for (rr=w1+14, ss=(&w506); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w506); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1105: @505 = (@205*@505) */ - w505 = (w205*w505); + w505 = (w205 * w505); /* #1106: (@1[13] += @505) */ - for (rr=w1+13, ss=(&w505); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w505); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1107: @504 = (@182*@504) */ - w504 = (w182*w504); + w504 = (w182 * w504); /* #1108: (@1[13] += @504) */ - for (rr=w1+13, ss=(&w504); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w504); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1109: @504 = (@181*@503) */ - w504 = (w181*w503); + w504 = (w181 * w503); /* #1110: (@1[17] += @504) */ - for (rr=w1+17, ss=(&w504); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w504); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1111: @503 = (@206*@503) */ - w503 = (w206*w503); + w503 = (w206 * w503); /* #1112: (@1[12] += @503) */ - for (rr=w1+12, ss=(&w503); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w503); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1113: @503 = (@180*@502) */ - w503 = (w180*w502); + w503 = (w180 * w502); /* #1114: (@1[16] += @503) */ - for (rr=w1+16, ss=(&w503); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w503); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1115: @502 = (@207*@502) */ - w502 = (w207*w502); + w502 = (w207 * w502); /* #1116: (@1[12] += @502) */ - for (rr=w1+12, ss=(&w502); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w502); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1117: @502 = (@179*@501) */ - w502 = (w179*w501); + w502 = (w179 * w501); /* #1118: (@1[15] += @502) */ - for (rr=w1+15, ss=(&w502); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w502); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1119: @501 = (@208*@501) */ - w501 = (w208*w501); + w501 = (w208 * w501); /* #1120: (@1[12] += @501) */ - for (rr=w1+12, ss=(&w501); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w501); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1121: @501 = (@178*@500) */ - w501 = (w178*w500); + w501 = (w178 * w500); /* #1122: (@1[14] += @501) */ - for (rr=w1+14, ss=(&w501); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w501); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1123: @500 = (@209*@500) */ - w500 = (w209*w500); + w500 = (w209 * w500); /* #1124: (@1[12] += @500) */ - for (rr=w1+12, ss=(&w500); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w500); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1125: @500 = (@177*@499) */ - w500 = (w177*w499); + w500 = (w177 * w499); /* #1126: (@1[13] += @500) */ - for (rr=w1+13, ss=(&w500); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w500); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1127: @499 = (@210*@499) */ - w499 = (w210*w499); + w499 = (w210 * w499); /* #1128: (@1[12] += @499) */ - for (rr=w1+12, ss=(&w499); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w499); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1129: @498 = (@176*@498) */ - w498 = (w176*w498); + w498 = (w176 * w498); /* #1130: (@1[12] += @498) */ - for (rr=w1+12, ss=(&w498); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w498); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1131: @498 = (@175*@497) */ - w498 = (w175*w497); + w498 = (w175 * w497); /* #1132: (@1[17] += @498) */ - for (rr=w1+17, ss=(&w498); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w498); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1133: @497 = (@211*@497) */ - w497 = (w211*w497); + w497 = (w211 * w497); /* #1134: (@1[11] += @497) */ - for (rr=w1+11, ss=(&w497); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w497); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1135: @497 = (@174*@496) */ - w497 = (w174*w496); + w497 = (w174 * w496); /* #1136: (@1[16] += @497) */ - for (rr=w1+16, ss=(&w497); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w497); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1137: @496 = (@212*@496) */ - w496 = (w212*w496); + w496 = (w212 * w496); /* #1138: (@1[11] += @496) */ - for (rr=w1+11, ss=(&w496); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w496); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1139: @496 = (@173*@495) */ - w496 = (w173*w495); + w496 = (w173 * w495); /* #1140: (@1[15] += @496) */ - for (rr=w1+15, ss=(&w496); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w496); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1141: @495 = (@213*@495) */ - w495 = (w213*w495); + w495 = (w213 * w495); /* #1142: (@1[11] += @495) */ - for (rr=w1+11, ss=(&w495); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w495); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1143: @495 = (@172*@494) */ - w495 = (w172*w494); + w495 = (w172 * w494); /* #1144: (@1[14] += @495) */ - for (rr=w1+14, ss=(&w495); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w495); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1145: @494 = (@214*@494) */ - w494 = (w214*w494); + w494 = (w214 * w494); /* #1146: (@1[11] += @494) */ - for (rr=w1+11, ss=(&w494); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w494); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1147: @494 = (@171*@493) */ - w494 = (w171*w493); + w494 = (w171 * w493); /* #1148: (@1[13] += @494) */ - for (rr=w1+13, ss=(&w494); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w494); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1149: @493 = (@215*@493) */ - w493 = (w215*w493); + w493 = (w215 * w493); /* #1150: (@1[11] += @493) */ - for (rr=w1+11, ss=(&w493); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w493); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1151: @493 = (@170*@492) */ - w493 = (w170*w492); + w493 = (w170 * w492); /* #1152: (@1[12] += @493) */ - for (rr=w1+12, ss=(&w493); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w493); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1153: @492 = (@216*@492) */ - w492 = (w216*w492); + w492 = (w216 * w492); /* #1154: (@1[11] += @492) */ - for (rr=w1+11, ss=(&w492); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w492); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1155: @491 = (@169*@491) */ - w491 = (w169*w491); + w491 = (w169 * w491); /* #1156: (@1[11] += @491) */ - for (rr=w1+11, ss=(&w491); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w491); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1157: @491 = (@168*@490) */ - w491 = (w168*w490); + w491 = (w168 * w490); /* #1158: (@1[17] += @491) */ - for (rr=w1+17, ss=(&w491); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w491); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1159: @490 = (@217*@490) */ - w490 = (w217*w490); + w490 = (w217 * w490); /* #1160: (@1[10] += @490) */ - for (rr=w1+10, ss=(&w490); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w490); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1161: @490 = (@167*@489) */ - w490 = (w167*w489); + w490 = (w167 * w489); /* #1162: (@1[16] += @490) */ - for (rr=w1+16, ss=(&w490); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w490); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1163: @489 = (@218*@489) */ - w489 = (w218*w489); + w489 = (w218 * w489); /* #1164: (@1[10] += @489) */ - for (rr=w1+10, ss=(&w489); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w489); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1165: @489 = (@166*@488) */ - w489 = (w166*w488); + w489 = (w166 * w488); /* #1166: (@1[15] += @489) */ - for (rr=w1+15, ss=(&w489); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w489); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1167: @488 = (@219*@488) */ - w488 = (w219*w488); + w488 = (w219 * w488); /* #1168: (@1[10] += @488) */ - for (rr=w1+10, ss=(&w488); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w488); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1169: @488 = (@165*@487) */ - w488 = (w165*w487); + w488 = (w165 * w487); /* #1170: (@1[14] += @488) */ - for (rr=w1+14, ss=(&w488); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w488); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1171: @487 = (@220*@487) */ - w487 = (w220*w487); + w487 = (w220 * w487); /* #1172: (@1[10] += @487) */ - for (rr=w1+10, ss=(&w487); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w487); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1173: @487 = (@164*@486) */ - w487 = (w164*w486); + w487 = (w164 * w486); /* #1174: (@1[13] += @487) */ - for (rr=w1+13, ss=(&w487); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w487); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1175: @486 = (@221*@486) */ - w486 = (w221*w486); + w486 = (w221 * w486); /* #1176: (@1[10] += @486) */ - for (rr=w1+10, ss=(&w486); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w486); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1177: @486 = (@163*@485) */ - w486 = (w163*w485); + w486 = (w163 * w485); /* #1178: (@1[12] += @486) */ - for (rr=w1+12, ss=(&w486); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w486); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1179: @485 = (@222*@485) */ - w485 = (w222*w485); + w485 = (w222 * w485); /* #1180: (@1[10] += @485) */ - for (rr=w1+10, ss=(&w485); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w485); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1181: @485 = (@162*@484) */ - w485 = (w162*w484); + w485 = (w162 * w484); /* #1182: (@1[11] += @485) */ - for (rr=w1+11, ss=(&w485); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w485); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1183: @484 = (@223*@484) */ - w484 = (w223*w484); + w484 = (w223 * w484); /* #1184: (@1[10] += @484) */ - for (rr=w1+10, ss=(&w484); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w484); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1185: @483 = (@161*@483) */ - w483 = (w161*w483); + w483 = (w161 * w483); /* #1186: (@1[10] += @483) */ - for (rr=w1+10, ss=(&w483); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w483); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1187: @483 = (@160*@482) */ - w483 = (w160*w482); + w483 = (w160 * w482); /* #1188: (@1[17] += @483) */ - for (rr=w1+17, ss=(&w483); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w483); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1189: @482 = (@224*@482) */ - w482 = (w224*w482); + w482 = (w224 * w482); /* #1190: (@1[9] += @482) */ - for (rr=w1+9, ss=(&w482); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w482); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1191: @482 = (@159*@481) */ - w482 = (w159*w481); + w482 = (w159 * w481); /* #1192: (@1[16] += @482) */ - for (rr=w1+16, ss=(&w482); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w482); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1193: @481 = (@225*@481) */ - w481 = (w225*w481); + w481 = (w225 * w481); /* #1194: (@1[9] += @481) */ - for (rr=w1+9, ss=(&w481); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w481); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1195: @481 = (@158*@480) */ - w481 = (w158*w480); + w481 = (w158 * w480); /* #1196: (@1[15] += @481) */ - for (rr=w1+15, ss=(&w481); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w481); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1197: @480 = (@226*@480) */ - w480 = (w226*w480); + w480 = (w226 * w480); /* #1198: (@1[9] += @480) */ - for (rr=w1+9, ss=(&w480); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w480); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1199: @480 = (@157*@479) */ - w480 = (w157*w479); + w480 = (w157 * w479); /* #1200: (@1[14] += @480) */ - for (rr=w1+14, ss=(&w480); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w480); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1201: @479 = (@227*@479) */ - w479 = (w227*w479); + w479 = (w227 * w479); /* #1202: (@1[9] += @479) */ - for (rr=w1+9, ss=(&w479); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w479); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1203: @479 = (@156*@478) */ - w479 = (w156*w478); + w479 = (w156 * w478); /* #1204: (@1[13] += @479) */ - for (rr=w1+13, ss=(&w479); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w479); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1205: @478 = (@228*@478) */ - w478 = (w228*w478); + w478 = (w228 * w478); /* #1206: (@1[9] += @478) */ - for (rr=w1+9, ss=(&w478); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w478); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1207: @478 = (@155*@477) */ - w478 = (w155*w477); + w478 = (w155 * w477); /* #1208: (@1[12] += @478) */ - for (rr=w1+12, ss=(&w478); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w478); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1209: @477 = (@229*@477) */ - w477 = (w229*w477); + w477 = (w229 * w477); /* #1210: (@1[9] += @477) */ - for (rr=w1+9, ss=(&w477); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w477); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1211: @477 = (@154*@476) */ - w477 = (w154*w476); + w477 = (w154 * w476); /* #1212: (@1[11] += @477) */ - for (rr=w1+11, ss=(&w477); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w477); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1213: @476 = (@230*@476) */ - w476 = (w230*w476); + w476 = (w230 * w476); /* #1214: (@1[9] += @476) */ - for (rr=w1+9, ss=(&w476); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w476); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1215: @476 = (@153*@475) */ - w476 = (w153*w475); + w476 = (w153 * w475); /* #1216: (@1[10] += @476) */ - for (rr=w1+10, ss=(&w476); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w476); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1217: @475 = (@231*@475) */ - w475 = (w231*w475); + w475 = (w231 * w475); /* #1218: (@1[9] += @475) */ - for (rr=w1+9, ss=(&w475); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w475); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1219: @474 = (@152*@474) */ - w474 = (w152*w474); + w474 = (w152 * w474); /* #1220: (@1[9] += @474) */ - for (rr=w1+9, ss=(&w474); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w474); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1221: @474 = (@151*@473) */ - w474 = (w151*w473); + w474 = (w151 * w473); /* #1222: (@1[17] += @474) */ - for (rr=w1+17, ss=(&w474); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w474); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1223: @473 = (@232*@473) */ - w473 = (w232*w473); + w473 = (w232 * w473); /* #1224: (@1[8] += @473) */ - for (rr=w1+8, ss=(&w473); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w473); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1225: @473 = (@150*@472) */ - w473 = (w150*w472); + w473 = (w150 * w472); /* #1226: (@1[16] += @473) */ - for (rr=w1+16, ss=(&w473); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w473); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1227: @472 = (@233*@472) */ - w472 = (w233*w472); + w472 = (w233 * w472); /* #1228: (@1[8] += @472) */ - for (rr=w1+8, ss=(&w472); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w472); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1229: @472 = (@149*@471) */ - w472 = (w149*w471); + w472 = (w149 * w471); /* #1230: (@1[15] += @472) */ - for (rr=w1+15, ss=(&w472); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w472); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1231: @471 = (@234*@471) */ - w471 = (w234*w471); + w471 = (w234 * w471); /* #1232: (@1[8] += @471) */ - for (rr=w1+8, ss=(&w471); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w471); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1233: @471 = (@148*@470) */ - w471 = (w148*w470); + w471 = (w148 * w470); /* #1234: (@1[14] += @471) */ - for (rr=w1+14, ss=(&w471); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w471); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1235: @470 = (@235*@470) */ - w470 = (w235*w470); + w470 = (w235 * w470); /* #1236: (@1[8] += @470) */ - for (rr=w1+8, ss=(&w470); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w470); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1237: @470 = (@147*@469) */ - w470 = (w147*w469); + w470 = (w147 * w469); /* #1238: (@1[13] += @470) */ - for (rr=w1+13, ss=(&w470); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w470); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1239: @469 = (@236*@469) */ - w469 = (w236*w469); + w469 = (w236 * w469); /* #1240: (@1[8] += @469) */ - for (rr=w1+8, ss=(&w469); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w469); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1241: @469 = (@146*@468) */ - w469 = (w146*w468); + w469 = (w146 * w468); /* #1242: (@1[12] += @469) */ - for (rr=w1+12, ss=(&w469); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w469); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1243: @468 = (@237*@468) */ - w468 = (w237*w468); + w468 = (w237 * w468); /* #1244: (@1[8] += @468) */ - for (rr=w1+8, ss=(&w468); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w468); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1245: @468 = (@145*@467) */ - w468 = (w145*w467); + w468 = (w145 * w467); /* #1246: (@1[11] += @468) */ - for (rr=w1+11, ss=(&w468); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w468); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1247: @467 = (@238*@467) */ - w467 = (w238*w467); + w467 = (w238 * w467); /* #1248: (@1[8] += @467) */ - for (rr=w1+8, ss=(&w467); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w467); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1249: @467 = (@144*@466) */ - w467 = (w144*w466); + w467 = (w144 * w466); /* #1250: (@1[10] += @467) */ - for (rr=w1+10, ss=(&w467); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w467); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1251: @466 = (@239*@466) */ - w466 = (w239*w466); + w466 = (w239 * w466); /* #1252: (@1[8] += @466) */ - for (rr=w1+8, ss=(&w466); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w466); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1253: @466 = (@143*@465) */ - w466 = (w143*w465); + w466 = (w143 * w465); /* #1254: (@1[9] += @466) */ - for (rr=w1+9, ss=(&w466); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w466); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1255: @465 = (@240*@465) */ - w465 = (w240*w465); + w465 = (w240 * w465); /* #1256: (@1[8] += @465) */ - for (rr=w1+8, ss=(&w465); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w465); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1257: @464 = (@142*@464) */ - w464 = (w142*w464); + w464 = (w142 * w464); /* #1258: (@1[8] += @464) */ - for (rr=w1+8, ss=(&w464); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w464); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1259: @464 = (@141*@463) */ - w464 = (w141*w463); + w464 = (w141 * w463); /* #1260: (@1[17] += @464) */ - for (rr=w1+17, ss=(&w464); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w464); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1261: @463 = (@241*@463) */ - w463 = (w241*w463); + w463 = (w241 * w463); /* #1262: (@1[7] += @463) */ - for (rr=w1+7, ss=(&w463); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w463); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1263: @463 = (@140*@462) */ - w463 = (w140*w462); + w463 = (w140 * w462); /* #1264: (@1[16] += @463) */ - for (rr=w1+16, ss=(&w463); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w463); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1265: @462 = (@242*@462) */ - w462 = (w242*w462); + w462 = (w242 * w462); /* #1266: (@1[7] += @462) */ - for (rr=w1+7, ss=(&w462); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w462); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1267: @462 = (@139*@461) */ - w462 = (w139*w461); + w462 = (w139 * w461); /* #1268: (@1[15] += @462) */ - for (rr=w1+15, ss=(&w462); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w462); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1269: @461 = (@243*@461) */ - w461 = (w243*w461); + w461 = (w243 * w461); /* #1270: (@1[7] += @461) */ - for (rr=w1+7, ss=(&w461); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w461); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1271: @461 = (@138*@460) */ - w461 = (w138*w460); + w461 = (w138 * w460); /* #1272: (@1[14] += @461) */ - for (rr=w1+14, ss=(&w461); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w461); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1273: @460 = (@244*@460) */ - w460 = (w244*w460); + w460 = (w244 * w460); /* #1274: (@1[7] += @460) */ - for (rr=w1+7, ss=(&w460); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w460); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1275: @460 = (@137*@459) */ - w460 = (w137*w459); + w460 = (w137 * w459); /* #1276: (@1[13] += @460) */ - for (rr=w1+13, ss=(&w460); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w460); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1277: @459 = (@245*@459) */ - w459 = (w245*w459); + w459 = (w245 * w459); /* #1278: (@1[7] += @459) */ - for (rr=w1+7, ss=(&w459); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w459); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1279: @459 = (@136*@458) */ - w459 = (w136*w458); + w459 = (w136 * w458); /* #1280: (@1[12] += @459) */ - for (rr=w1+12, ss=(&w459); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w459); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1281: @458 = (@246*@458) */ - w458 = (w246*w458); + w458 = (w246 * w458); /* #1282: (@1[7] += @458) */ - for (rr=w1+7, ss=(&w458); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w458); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1283: @458 = (@135*@457) */ - w458 = (w135*w457); + w458 = (w135 * w457); /* #1284: (@1[11] += @458) */ - for (rr=w1+11, ss=(&w458); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w458); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1285: @457 = (@247*@457) */ - w457 = (w247*w457); + w457 = (w247 * w457); /* #1286: (@1[7] += @457) */ - for (rr=w1+7, ss=(&w457); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w457); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1287: @457 = (@134*@456) */ - w457 = (w134*w456); + w457 = (w134 * w456); /* #1288: (@1[10] += @457) */ - for (rr=w1+10, ss=(&w457); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w457); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1289: @456 = (@248*@456) */ - w456 = (w248*w456); + w456 = (w248 * w456); /* #1290: (@1[7] += @456) */ - for (rr=w1+7, ss=(&w456); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w456); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1291: @456 = (@133*@455) */ - w456 = (w133*w455); + w456 = (w133 * w455); /* #1292: (@1[9] += @456) */ - for (rr=w1+9, ss=(&w456); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w456); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1293: @455 = (@249*@455) */ - w455 = (w249*w455); + w455 = (w249 * w455); /* #1294: (@1[7] += @455) */ - for (rr=w1+7, ss=(&w455); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w455); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1295: @455 = (@132*@454) */ - w455 = (w132*w454); + w455 = (w132 * w454); /* #1296: (@1[8] += @455) */ - for (rr=w1+8, ss=(&w455); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w455); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1297: @454 = (@250*@454) */ - w454 = (w250*w454); + w454 = (w250 * w454); /* #1298: (@1[7] += @454) */ - for (rr=w1+7, ss=(&w454); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w454); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1299: @453 = (@131*@453) */ - w453 = (w131*w453); + w453 = (w131 * w453); /* #1300: (@1[7] += @453) */ - for (rr=w1+7, ss=(&w453); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w453); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1301: @453 = (@130*@452) */ - w453 = (w130*w452); + w453 = (w130 * w452); /* #1302: (@1[17] += @453) */ - for (rr=w1+17, ss=(&w453); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w453); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1303: @452 = (@251*@452) */ - w452 = (w251*w452); + w452 = (w251 * w452); /* #1304: (@1[6] += @452) */ - for (rr=w1+6, ss=(&w452); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w452); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1305: @452 = (@129*@451) */ - w452 = (w129*w451); + w452 = (w129 * w451); /* #1306: (@1[16] += @452) */ - for (rr=w1+16, ss=(&w452); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w452); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1307: @451 = (@252*@451) */ - w451 = (w252*w451); + w451 = (w252 * w451); /* #1308: (@1[6] += @451) */ - for (rr=w1+6, ss=(&w451); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w451); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1309: @451 = (@128*@450) */ - w451 = (w128*w450); + w451 = (w128 * w450); /* #1310: (@1[15] += @451) */ - for (rr=w1+15, ss=(&w451); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w451); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1311: @450 = (@253*@450) */ - w450 = (w253*w450); + w450 = (w253 * w450); /* #1312: (@1[6] += @450) */ - for (rr=w1+6, ss=(&w450); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w450); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1313: @450 = (@127*@449) */ - w450 = (w127*w449); + w450 = (w127 * w449); /* #1314: (@1[14] += @450) */ - for (rr=w1+14, ss=(&w450); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w450); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1315: @449 = (@254*@449) */ - w449 = (w254*w449); + w449 = (w254 * w449); /* #1316: (@1[6] += @449) */ - for (rr=w1+6, ss=(&w449); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w449); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1317: @449 = (@126*@448) */ - w449 = (w126*w448); + w449 = (w126 * w448); /* #1318: (@1[13] += @449) */ - for (rr=w1+13, ss=(&w449); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w449); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1319: @448 = (@255*@448) */ - w448 = (w255*w448); + w448 = (w255 * w448); /* #1320: (@1[6] += @448) */ - for (rr=w1+6, ss=(&w448); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w448); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1321: @448 = (@125*@447) */ - w448 = (w125*w447); + w448 = (w125 * w447); /* #1322: (@1[12] += @448) */ - for (rr=w1+12, ss=(&w448); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w448); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1323: @447 = (@256*@447) */ - w447 = (w256*w447); + w447 = (w256 * w447); /* #1324: (@1[6] += @447) */ - for (rr=w1+6, ss=(&w447); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w447); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1325: @447 = (@124*@446) */ - w447 = (w124*w446); + w447 = (w124 * w446); /* #1326: (@1[11] += @447) */ - for (rr=w1+11, ss=(&w447); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w447); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1327: @446 = (@257*@446) */ - w446 = (w257*w446); + w446 = (w257 * w446); /* #1328: (@1[6] += @446) */ - for (rr=w1+6, ss=(&w446); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w446); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1329: @446 = (@123*@445) */ - w446 = (w123*w445); + w446 = (w123 * w445); /* #1330: (@1[10] += @446) */ - for (rr=w1+10, ss=(&w446); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w446); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1331: @445 = (@258*@445) */ - w445 = (w258*w445); + w445 = (w258 * w445); /* #1332: (@1[6] += @445) */ - for (rr=w1+6, ss=(&w445); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w445); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1333: @445 = (@122*@444) */ - w445 = (w122*w444); + w445 = (w122 * w444); /* #1334: (@1[9] += @445) */ - for (rr=w1+9, ss=(&w445); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w445); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1335: @444 = (@259*@444) */ - w444 = (w259*w444); + w444 = (w259 * w444); /* #1336: (@1[6] += @444) */ - for (rr=w1+6, ss=(&w444); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w444); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1337: @444 = (@121*@443) */ - w444 = (w121*w443); + w444 = (w121 * w443); /* #1338: (@1[8] += @444) */ - for (rr=w1+8, ss=(&w444); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w444); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1339: @443 = (@260*@443) */ - w443 = (w260*w443); + w443 = (w260 * w443); /* #1340: (@1[6] += @443) */ - for (rr=w1+6, ss=(&w443); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w443); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1341: @443 = (@120*@442) */ - w443 = (w120*w442); + w443 = (w120 * w442); /* #1342: (@1[7] += @443) */ - for (rr=w1+7, ss=(&w443); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w443); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1343: @442 = (@261*@442) */ - w442 = (w261*w442); + w442 = (w261 * w442); /* #1344: (@1[6] += @442) */ - for (rr=w1+6, ss=(&w442); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w442); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1345: @441 = (@119*@441) */ - w441 = (w119*w441); + w441 = (w119 * w441); /* #1346: (@1[6] += @441) */ - for (rr=w1+6, ss=(&w441); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w441); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1347: @441 = (@118*@440) */ - w441 = (w118*w440); + w441 = (w118 * w440); /* #1348: (@1[17] += @441) */ - for (rr=w1+17, ss=(&w441); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w441); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1349: @440 = (@262*@440) */ - w440 = (w262*w440); + w440 = (w262 * w440); /* #1350: (@1[5] += @440) */ - for (rr=w1+5, ss=(&w440); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w440); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1351: @440 = (@117*@439) */ - w440 = (w117*w439); + w440 = (w117 * w439); /* #1352: (@1[16] += @440) */ - for (rr=w1+16, ss=(&w440); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w440); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1353: @439 = (@263*@439) */ - w439 = (w263*w439); + w439 = (w263 * w439); /* #1354: (@1[5] += @439) */ - for (rr=w1+5, ss=(&w439); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w439); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1355: @439 = (@116*@438) */ - w439 = (w116*w438); + w439 = (w116 * w438); /* #1356: (@1[15] += @439) */ - for (rr=w1+15, ss=(&w439); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w439); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1357: @438 = (@264*@438) */ - w438 = (w264*w438); + w438 = (w264 * w438); /* #1358: (@1[5] += @438) */ - for (rr=w1+5, ss=(&w438); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w438); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1359: @438 = (@115*@437) */ - w438 = (w115*w437); + w438 = (w115 * w437); /* #1360: (@1[14] += @438) */ - for (rr=w1+14, ss=(&w438); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w438); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1361: @437 = (@265*@437) */ - w437 = (w265*w437); + w437 = (w265 * w437); /* #1362: (@1[5] += @437) */ - for (rr=w1+5, ss=(&w437); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w437); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1363: @437 = (@114*@436) */ - w437 = (w114*w436); + w437 = (w114 * w436); /* #1364: (@1[13] += @437) */ - for (rr=w1+13, ss=(&w437); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w437); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1365: @436 = (@266*@436) */ - w436 = (w266*w436); + w436 = (w266 * w436); /* #1366: (@1[5] += @436) */ - for (rr=w1+5, ss=(&w436); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w436); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1367: @436 = (@113*@435) */ - w436 = (w113*w435); + w436 = (w113 * w435); /* #1368: (@1[12] += @436) */ - for (rr=w1+12, ss=(&w436); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w436); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1369: @435 = (@267*@435) */ - w435 = (w267*w435); + w435 = (w267 * w435); /* #1370: (@1[5] += @435) */ - for (rr=w1+5, ss=(&w435); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w435); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1371: @435 = (@112*@434) */ - w435 = (w112*w434); + w435 = (w112 * w434); /* #1372: (@1[11] += @435) */ - for (rr=w1+11, ss=(&w435); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w435); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1373: @434 = (@268*@434) */ - w434 = (w268*w434); + w434 = (w268 * w434); /* #1374: (@1[5] += @434) */ - for (rr=w1+5, ss=(&w434); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w434); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1375: @434 = (@111*@433) */ - w434 = (w111*w433); + w434 = (w111 * w433); /* #1376: (@1[10] += @434) */ - for (rr=w1+10, ss=(&w434); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w434); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1377: @433 = (@269*@433) */ - w433 = (w269*w433); + w433 = (w269 * w433); /* #1378: (@1[5] += @433) */ - for (rr=w1+5, ss=(&w433); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w433); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1379: @433 = (@110*@432) */ - w433 = (w110*w432); + w433 = (w110 * w432); /* #1380: (@1[9] += @433) */ - for (rr=w1+9, ss=(&w433); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w433); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1381: @432 = (@270*@432) */ - w432 = (w270*w432); + w432 = (w270 * w432); /* #1382: (@1[5] += @432) */ - for (rr=w1+5, ss=(&w432); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w432); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1383: @432 = (@109*@431) */ - w432 = (w109*w431); + w432 = (w109 * w431); /* #1384: (@1[8] += @432) */ - for (rr=w1+8, ss=(&w432); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w432); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1385: @431 = (@271*@431) */ - w431 = (w271*w431); + w431 = (w271 * w431); /* #1386: (@1[5] += @431) */ - for (rr=w1+5, ss=(&w431); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w431); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1387: @431 = (@108*@430) */ - w431 = (w108*w430); + w431 = (w108 * w430); /* #1388: (@1[7] += @431) */ - for (rr=w1+7, ss=(&w431); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w431); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1389: @430 = (@272*@430) */ - w430 = (w272*w430); + w430 = (w272 * w430); /* #1390: (@1[5] += @430) */ - for (rr=w1+5, ss=(&w430); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w430); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1391: @430 = (@107*@429) */ - w430 = (w107*w429); + w430 = (w107 * w429); /* #1392: (@1[6] += @430) */ - for (rr=w1+6, ss=(&w430); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w430); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1393: @429 = (@273*@429) */ - w429 = (w273*w429); + w429 = (w273 * w429); /* #1394: (@1[5] += @429) */ - for (rr=w1+5, ss=(&w429); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w429); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1395: @428 = (@106*@428) */ - w428 = (w106*w428); + w428 = (w106 * w428); /* #1396: (@1[5] += @428) */ - for (rr=w1+5, ss=(&w428); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w428); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1397: @428 = (@105*@427) */ - w428 = (w105*w427); + w428 = (w105 * w427); /* #1398: (@1[17] += @428) */ - for (rr=w1+17, ss=(&w428); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w428); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1399: @427 = (@274*@427) */ - w427 = (w274*w427); + w427 = (w274 * w427); /* #1400: (@1[4] += @427) */ - for (rr=w1+4, ss=(&w427); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w427); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1401: @427 = (@104*@426) */ - w427 = (w104*w426); + w427 = (w104 * w426); /* #1402: (@1[16] += @427) */ - for (rr=w1+16, ss=(&w427); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w427); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1403: @426 = (@275*@426) */ - w426 = (w275*w426); + w426 = (w275 * w426); /* #1404: (@1[4] += @426) */ - for (rr=w1+4, ss=(&w426); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w426); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1405: @426 = (@103*@425) */ - w426 = (w103*w425); + w426 = (w103 * w425); /* #1406: (@1[15] += @426) */ - for (rr=w1+15, ss=(&w426); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w426); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1407: @425 = (@276*@425) */ - w425 = (w276*w425); + w425 = (w276 * w425); /* #1408: (@1[4] += @425) */ - for (rr=w1+4, ss=(&w425); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w425); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1409: @425 = (@102*@424) */ - w425 = (w102*w424); + w425 = (w102 * w424); /* #1410: (@1[14] += @425) */ - for (rr=w1+14, ss=(&w425); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w425); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1411: @424 = (@277*@424) */ - w424 = (w277*w424); + w424 = (w277 * w424); /* #1412: (@1[4] += @424) */ - for (rr=w1+4, ss=(&w424); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w424); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1413: @424 = (@101*@423) */ - w424 = (w101*w423); + w424 = (w101 * w423); /* #1414: (@1[13] += @424) */ - for (rr=w1+13, ss=(&w424); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w424); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1415: @423 = (@278*@423) */ - w423 = (w278*w423); + w423 = (w278 * w423); /* #1416: (@1[4] += @423) */ - for (rr=w1+4, ss=(&w423); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w423); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1417: @423 = (@100*@422) */ - w423 = (w100*w422); + w423 = (w100 * w422); /* #1418: (@1[12] += @423) */ - for (rr=w1+12, ss=(&w423); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w423); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1419: @422 = (@279*@422) */ - w422 = (w279*w422); + w422 = (w279 * w422); /* #1420: (@1[4] += @422) */ - for (rr=w1+4, ss=(&w422); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w422); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1421: @422 = (@99*@421) */ - w422 = (w99*w421); + w422 = (w99 * w421); /* #1422: (@1[11] += @422) */ - for (rr=w1+11, ss=(&w422); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w422); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1423: @421 = (@280*@421) */ - w421 = (w280*w421); + w421 = (w280 * w421); /* #1424: (@1[4] += @421) */ - for (rr=w1+4, ss=(&w421); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w421); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1425: @421 = (@98*@420) */ - w421 = (w98*w420); + w421 = (w98 * w420); /* #1426: (@1[10] += @421) */ - for (rr=w1+10, ss=(&w421); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w421); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1427: @420 = (@281*@420) */ - w420 = (w281*w420); + w420 = (w281 * w420); /* #1428: (@1[4] += @420) */ - for (rr=w1+4, ss=(&w420); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w420); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1429: @420 = (@97*@419) */ - w420 = (w97*w419); + w420 = (w97 * w419); /* #1430: (@1[9] += @420) */ - for (rr=w1+9, ss=(&w420); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w420); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1431: @419 = (@282*@419) */ - w419 = (w282*w419); + w419 = (w282 * w419); /* #1432: (@1[4] += @419) */ - for (rr=w1+4, ss=(&w419); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w419); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1433: @419 = (@96*@418) */ - w419 = (w96*w418); + w419 = (w96 * w418); /* #1434: (@1[8] += @419) */ - for (rr=w1+8, ss=(&w419); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w419); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1435: @418 = (@283*@418) */ - w418 = (w283*w418); + w418 = (w283 * w418); /* #1436: (@1[4] += @418) */ - for (rr=w1+4, ss=(&w418); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w418); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1437: @418 = (@95*@417) */ - w418 = (w95*w417); + w418 = (w95 * w417); /* #1438: (@1[7] += @418) */ - for (rr=w1+7, ss=(&w418); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w418); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1439: @417 = (@284*@417) */ - w417 = (w284*w417); + w417 = (w284 * w417); /* #1440: (@1[4] += @417) */ - for (rr=w1+4, ss=(&w417); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w417); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1441: @417 = (@94*@416) */ - w417 = (w94*w416); + w417 = (w94 * w416); /* #1442: (@1[6] += @417) */ - for (rr=w1+6, ss=(&w417); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w417); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1443: @416 = (@285*@416) */ - w416 = (w285*w416); + w416 = (w285 * w416); /* #1444: (@1[4] += @416) */ - for (rr=w1+4, ss=(&w416); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w416); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1445: @416 = (@93*@415) */ - w416 = (w93*w415); + w416 = (w93 * w415); /* #1446: (@1[5] += @416) */ - for (rr=w1+5, ss=(&w416); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w416); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1447: @415 = (@286*@415) */ - w415 = (w286*w415); + w415 = (w286 * w415); /* #1448: (@1[4] += @415) */ - for (rr=w1+4, ss=(&w415); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w415); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1449: @414 = (@92*@414) */ - w414 = (w92*w414); + w414 = (w92 * w414); /* #1450: (@1[4] += @414) */ - for (rr=w1+4, ss=(&w414); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w414); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1451: @414 = (@91*@413) */ - w414 = (w91*w413); + w414 = (w91 * w413); /* #1452: (@1[17] += @414) */ - for (rr=w1+17, ss=(&w414); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w414); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1453: @413 = (@287*@413) */ - w413 = (w287*w413); + w413 = (w287 * w413); /* #1454: (@1[3] += @413) */ - for (rr=w1+3, ss=(&w413); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w413); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1455: @413 = (@90*@412) */ - w413 = (w90*w412); + w413 = (w90 * w412); /* #1456: (@1[16] += @413) */ - for (rr=w1+16, ss=(&w413); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w413); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1457: @412 = (@288*@412) */ - w412 = (w288*w412); + w412 = (w288 * w412); /* #1458: (@1[3] += @412) */ - for (rr=w1+3, ss=(&w412); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w412); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1459: @412 = (@89*@411) */ - w412 = (w89*w411); + w412 = (w89 * w411); /* #1460: (@1[15] += @412) */ - for (rr=w1+15, ss=(&w412); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w412); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1461: @411 = (@289*@411) */ - w411 = (w289*w411); + w411 = (w289 * w411); /* #1462: (@1[3] += @411) */ - for (rr=w1+3, ss=(&w411); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w411); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1463: @411 = (@88*@410) */ - w411 = (w88*w410); + w411 = (w88 * w410); /* #1464: (@1[14] += @411) */ - for (rr=w1+14, ss=(&w411); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w411); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1465: @410 = (@290*@410) */ - w410 = (w290*w410); + w410 = (w290 * w410); /* #1466: (@1[3] += @410) */ - for (rr=w1+3, ss=(&w410); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w410); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1467: @410 = (@87*@409) */ - w410 = (w87*w409); + w410 = (w87 * w409); /* #1468: (@1[13] += @410) */ - for (rr=w1+13, ss=(&w410); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w410); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1469: @409 = (@291*@409) */ - w409 = (w291*w409); + w409 = (w291 * w409); /* #1470: (@1[3] += @409) */ - for (rr=w1+3, ss=(&w409); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w409); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1471: @409 = (@86*@408) */ - w409 = (w86*w408); + w409 = (w86 * w408); /* #1472: (@1[12] += @409) */ - for (rr=w1+12, ss=(&w409); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w409); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1473: @408 = (@292*@408) */ - w408 = (w292*w408); + w408 = (w292 * w408); /* #1474: (@1[3] += @408) */ - for (rr=w1+3, ss=(&w408); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w408); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1475: @408 = (@85*@407) */ - w408 = (w85*w407); + w408 = (w85 * w407); /* #1476: (@1[11] += @408) */ - for (rr=w1+11, ss=(&w408); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w408); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1477: @407 = (@293*@407) */ - w407 = (w293*w407); + w407 = (w293 * w407); /* #1478: (@1[3] += @407) */ - for (rr=w1+3, ss=(&w407); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w407); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1479: @407 = (@84*@406) */ - w407 = (w84*w406); + w407 = (w84 * w406); /* #1480: (@1[10] += @407) */ - for (rr=w1+10, ss=(&w407); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w407); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1481: @406 = (@294*@406) */ - w406 = (w294*w406); + w406 = (w294 * w406); /* #1482: (@1[3] += @406) */ - for (rr=w1+3, ss=(&w406); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w406); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1483: @406 = (@83*@405) */ - w406 = (w83*w405); + w406 = (w83 * w405); /* #1484: (@1[9] += @406) */ - for (rr=w1+9, ss=(&w406); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w406); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1485: @405 = (@295*@405) */ - w405 = (w295*w405); + w405 = (w295 * w405); /* #1486: (@1[3] += @405) */ - for (rr=w1+3, ss=(&w405); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w405); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1487: @405 = (@82*@404) */ - w405 = (w82*w404); + w405 = (w82 * w404); /* #1488: (@1[8] += @405) */ - for (rr=w1+8, ss=(&w405); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w405); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1489: @404 = (@296*@404) */ - w404 = (w296*w404); + w404 = (w296 * w404); /* #1490: (@1[3] += @404) */ - for (rr=w1+3, ss=(&w404); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w404); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1491: @404 = (@81*@403) */ - w404 = (w81*w403); + w404 = (w81 * w403); /* #1492: (@1[7] += @404) */ - for (rr=w1+7, ss=(&w404); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w404); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1493: @403 = (@297*@403) */ - w403 = (w297*w403); + w403 = (w297 * w403); /* #1494: (@1[3] += @403) */ - for (rr=w1+3, ss=(&w403); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w403); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1495: @403 = (@80*@402) */ - w403 = (w80*w402); + w403 = (w80 * w402); /* #1496: (@1[6] += @403) */ - for (rr=w1+6, ss=(&w403); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w403); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1497: @402 = (@298*@402) */ - w402 = (w298*w402); + w402 = (w298 * w402); /* #1498: (@1[3] += @402) */ - for (rr=w1+3, ss=(&w402); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w402); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1499: @402 = (@79*@401) */ - w402 = (w79*w401); + w402 = (w79 * w401); /* #1500: (@1[5] += @402) */ - for (rr=w1+5, ss=(&w402); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w402); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1501: @401 = (@299*@401) */ - w401 = (w299*w401); + w401 = (w299 * w401); /* #1502: (@1[3] += @401) */ - for (rr=w1+3, ss=(&w401); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w401); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1503: @401 = (@78*@400) */ - w401 = (w78*w400); + w401 = (w78 * w400); /* #1504: (@1[4] += @401) */ - for (rr=w1+4, ss=(&w401); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w401); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1505: @400 = (@300*@400) */ - w400 = (w300*w400); + w400 = (w300 * w400); /* #1506: (@1[3] += @400) */ - for (rr=w1+3, ss=(&w400); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w400); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1507: @399 = (@77*@399) */ - w399 = (w77*w399); + w399 = (w77 * w399); /* #1508: (@1[3] += @399) */ - for (rr=w1+3, ss=(&w399); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w399); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1509: @399 = (@76*@398) */ - w399 = (w76*w398); + w399 = (w76 * w398); /* #1510: (@1[17] += @399) */ - for (rr=w1+17, ss=(&w399); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w399); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1511: @398 = (@301*@398) */ - w398 = (w301*w398); + w398 = (w301 * w398); /* #1512: (@1[2] += @398) */ - for (rr=w1+2, ss=(&w398); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w398); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1513: @398 = (@75*@397) */ - w398 = (w75*w397); + w398 = (w75 * w397); /* #1514: (@1[16] += @398) */ - for (rr=w1+16, ss=(&w398); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w398); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1515: @397 = (@302*@397) */ - w397 = (w302*w397); + w397 = (w302 * w397); /* #1516: (@1[2] += @397) */ - for (rr=w1+2, ss=(&w397); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w397); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1517: @397 = (@74*@396) */ - w397 = (w74*w396); + w397 = (w74 * w396); /* #1518: (@1[15] += @397) */ - for (rr=w1+15, ss=(&w397); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w397); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1519: @396 = (@303*@396) */ - w396 = (w303*w396); + w396 = (w303 * w396); /* #1520: (@1[2] += @396) */ - for (rr=w1+2, ss=(&w396); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w396); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1521: @396 = (@73*@395) */ - w396 = (w73*w395); + w396 = (w73 * w395); /* #1522: (@1[14] += @396) */ - for (rr=w1+14, ss=(&w396); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w396); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1523: @395 = (@304*@395) */ - w395 = (w304*w395); + w395 = (w304 * w395); /* #1524: (@1[2] += @395) */ - for (rr=w1+2, ss=(&w395); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w395); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1525: @395 = (@72*@394) */ - w395 = (w72*w394); + w395 = (w72 * w394); /* #1526: (@1[13] += @395) */ - for (rr=w1+13, ss=(&w395); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w395); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1527: @394 = (@305*@394) */ - w394 = (w305*w394); + w394 = (w305 * w394); /* #1528: (@1[2] += @394) */ - for (rr=w1+2, ss=(&w394); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w394); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1529: @394 = (@71*@393) */ - w394 = (w71*w393); + w394 = (w71 * w393); /* #1530: (@1[12] += @394) */ - for (rr=w1+12, ss=(&w394); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w394); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1531: @393 = (@306*@393) */ - w393 = (w306*w393); + w393 = (w306 * w393); /* #1532: (@1[2] += @393) */ - for (rr=w1+2, ss=(&w393); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w393); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1533: @393 = (@70*@392) */ - w393 = (w70*w392); + w393 = (w70 * w392); /* #1534: (@1[11] += @393) */ - for (rr=w1+11, ss=(&w393); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w393); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1535: @392 = (@307*@392) */ - w392 = (w307*w392); + w392 = (w307 * w392); /* #1536: (@1[2] += @392) */ - for (rr=w1+2, ss=(&w392); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w392); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1537: @392 = (@69*@391) */ - w392 = (w69*w391); + w392 = (w69 * w391); /* #1538: (@1[10] += @392) */ - for (rr=w1+10, ss=(&w392); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w392); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1539: @391 = (@308*@391) */ - w391 = (w308*w391); + w391 = (w308 * w391); /* #1540: (@1[2] += @391) */ - for (rr=w1+2, ss=(&w391); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w391); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1541: @391 = (@68*@390) */ - w391 = (w68*w390); + w391 = (w68 * w390); /* #1542: (@1[9] += @391) */ - for (rr=w1+9, ss=(&w391); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w391); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1543: @390 = (@309*@390) */ - w390 = (w309*w390); + w390 = (w309 * w390); /* #1544: (@1[2] += @390) */ - for (rr=w1+2, ss=(&w390); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w390); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1545: @390 = (@67*@389) */ - w390 = (w67*w389); + w390 = (w67 * w389); /* #1546: (@1[8] += @390) */ - for (rr=w1+8, ss=(&w390); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w390); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1547: @389 = (@310*@389) */ - w389 = (w310*w389); + w389 = (w310 * w389); /* #1548: (@1[2] += @389) */ - for (rr=w1+2, ss=(&w389); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w389); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1549: @389 = (@66*@388) */ - w389 = (w66*w388); + w389 = (w66 * w388); /* #1550: (@1[7] += @389) */ - for (rr=w1+7, ss=(&w389); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w389); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1551: @388 = (@311*@388) */ - w388 = (w311*w388); + w388 = (w311 * w388); /* #1552: (@1[2] += @388) */ - for (rr=w1+2, ss=(&w388); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w388); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1553: @388 = (@65*@387) */ - w388 = (w65*w387); + w388 = (w65 * w387); /* #1554: (@1[6] += @388) */ - for (rr=w1+6, ss=(&w388); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w388); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1555: @387 = (@312*@387) */ - w387 = (w312*w387); + w387 = (w312 * w387); /* #1556: (@1[2] += @387) */ - for (rr=w1+2, ss=(&w387); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w387); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1557: @387 = (@64*@386) */ - w387 = (w64*w386); + w387 = (w64 * w386); /* #1558: (@1[5] += @387) */ - for (rr=w1+5, ss=(&w387); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w387); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1559: @386 = (@313*@386) */ - w386 = (w313*w386); + w386 = (w313 * w386); /* #1560: (@1[2] += @386) */ - for (rr=w1+2, ss=(&w386); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w386); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1561: @386 = (@63*@385) */ - w386 = (w63*w385); + w386 = (w63 * w385); /* #1562: (@1[4] += @386) */ - for (rr=w1+4, ss=(&w386); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w386); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1563: @385 = (@314*@385) */ - w385 = (w314*w385); + w385 = (w314 * w385); /* #1564: (@1[2] += @385) */ - for (rr=w1+2, ss=(&w385); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w385); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1565: @385 = (@62*@384) */ - w385 = (w62*w384); + w385 = (w62 * w384); /* #1566: (@1[3] += @385) */ - for (rr=w1+3, ss=(&w385); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w385); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1567: @384 = (@315*@384) */ - w384 = (w315*w384); + w384 = (w315 * w384); /* #1568: (@1[2] += @384) */ - for (rr=w1+2, ss=(&w384); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w384); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1569: @383 = (@61*@383) */ - w383 = (w61*w383); + w383 = (w61 * w383); /* #1570: (@1[2] += @383) */ - for (rr=w1+2, ss=(&w383); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w383); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1571: @383 = (@60*@382) */ - w383 = (w60*w382); + w383 = (w60 * w382); /* #1572: (@1[17] += @383) */ - for (rr=w1+17, ss=(&w383); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w383); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1573: @382 = (@316*@382) */ - w382 = (w316*w382); + w382 = (w316 * w382); /* #1574: (@1[1] += @382) */ - for (rr=w1+1, ss=(&w382); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w382); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1575: @382 = (@59*@381) */ - w382 = (w59*w381); + w382 = (w59 * w381); /* #1576: (@1[16] += @382) */ - for (rr=w1+16, ss=(&w382); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w382); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1577: @381 = (@317*@381) */ - w381 = (w317*w381); + w381 = (w317 * w381); /* #1578: (@1[1] += @381) */ - for (rr=w1+1, ss=(&w381); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w381); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1579: @381 = (@58*@380) */ - w381 = (w58*w380); + w381 = (w58 * w380); /* #1580: (@1[15] += @381) */ - for (rr=w1+15, ss=(&w381); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w381); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1581: @380 = (@318*@380) */ - w380 = (w318*w380); + w380 = (w318 * w380); /* #1582: (@1[1] += @380) */ - for (rr=w1+1, ss=(&w380); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w380); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1583: @380 = (@57*@379) */ - w380 = (w57*w379); + w380 = (w57 * w379); /* #1584: (@1[14] += @380) */ - for (rr=w1+14, ss=(&w380); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w380); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1585: @379 = (@319*@379) */ - w379 = (w319*w379); + w379 = (w319 * w379); /* #1586: (@1[1] += @379) */ - for (rr=w1+1, ss=(&w379); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w379); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1587: @379 = (@56*@378) */ - w379 = (w56*w378); + w379 = (w56 * w378); /* #1588: (@1[13] += @379) */ - for (rr=w1+13, ss=(&w379); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w379); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1589: @378 = (@320*@378) */ - w378 = (w320*w378); + w378 = (w320 * w378); /* #1590: (@1[1] += @378) */ - for (rr=w1+1, ss=(&w378); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w378); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1591: @378 = (@55*@377) */ - w378 = (w55*w377); + w378 = (w55 * w377); /* #1592: (@1[12] += @378) */ - for (rr=w1+12, ss=(&w378); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w378); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1593: @377 = (@321*@377) */ - w377 = (w321*w377); + w377 = (w321 * w377); /* #1594: (@1[1] += @377) */ - for (rr=w1+1, ss=(&w377); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w377); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1595: @377 = (@54*@376) */ - w377 = (w54*w376); + w377 = (w54 * w376); /* #1596: (@1[11] += @377) */ - for (rr=w1+11, ss=(&w377); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w377); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1597: @376 = (@322*@376) */ - w376 = (w322*w376); + w376 = (w322 * w376); /* #1598: (@1[1] += @376) */ - for (rr=w1+1, ss=(&w376); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w376); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1599: @376 = (@53*@375) */ - w376 = (w53*w375); + w376 = (w53 * w375); /* #1600: (@1[10] += @376) */ - for (rr=w1+10, ss=(&w376); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w376); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1601: @375 = (@323*@375) */ - w375 = (w323*w375); + w375 = (w323 * w375); /* #1602: (@1[1] += @375) */ - for (rr=w1+1, ss=(&w375); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w375); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1603: @375 = (@52*@374) */ - w375 = (w52*w374); + w375 = (w52 * w374); /* #1604: (@1[9] += @375) */ - for (rr=w1+9, ss=(&w375); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w375); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1605: @374 = (@324*@374) */ - w374 = (w324*w374); + w374 = (w324 * w374); /* #1606: (@1[1] += @374) */ - for (rr=w1+1, ss=(&w374); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w374); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1607: @374 = (@51*@373) */ - w374 = (w51*w373); + w374 = (w51 * w373); /* #1608: (@1[8] += @374) */ - for (rr=w1+8, ss=(&w374); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w374); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1609: @373 = (@325*@373) */ - w373 = (w325*w373); + w373 = (w325 * w373); /* #1610: (@1[1] += @373) */ - for (rr=w1+1, ss=(&w373); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w373); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1611: @373 = (@50*@372) */ - w373 = (w50*w372); + w373 = (w50 * w372); /* #1612: (@1[7] += @373) */ - for (rr=w1+7, ss=(&w373); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w373); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1613: @372 = (@326*@372) */ - w372 = (w326*w372); + w372 = (w326 * w372); /* #1614: (@1[1] += @372) */ - for (rr=w1+1, ss=(&w372); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w372); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1615: @372 = (@49*@371) */ - w372 = (w49*w371); + w372 = (w49 * w371); /* #1616: (@1[6] += @372) */ - for (rr=w1+6, ss=(&w372); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w372); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1617: @371 = (@327*@371) */ - w371 = (w327*w371); + w371 = (w327 * w371); /* #1618: (@1[1] += @371) */ - for (rr=w1+1, ss=(&w371); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w371); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1619: @371 = (@48*@370) */ - w371 = (w48*w370); + w371 = (w48 * w370); /* #1620: (@1[5] += @371) */ - for (rr=w1+5, ss=(&w371); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w371); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1621: @370 = (@328*@370) */ - w370 = (w328*w370); + w370 = (w328 * w370); /* #1622: (@1[1] += @370) */ - for (rr=w1+1, ss=(&w370); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w370); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1623: @370 = (@47*@369) */ - w370 = (w47*w369); + w370 = (w47 * w369); /* #1624: (@1[4] += @370) */ - for (rr=w1+4, ss=(&w370); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w370); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1625: @369 = (@329*@369) */ - w369 = (w329*w369); + w369 = (w329 * w369); /* #1626: (@1[1] += @369) */ - for (rr=w1+1, ss=(&w369); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w369); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1627: @369 = (@46*@368) */ - w369 = (w46*w368); + w369 = (w46 * w368); /* #1628: (@1[3] += @369) */ - for (rr=w1+3, ss=(&w369); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w369); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1629: @368 = (@330*@368) */ - w368 = (w330*w368); + w368 = (w330 * w368); /* #1630: (@1[1] += @368) */ - for (rr=w1+1, ss=(&w368); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w368); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1631: @368 = (@45*@367) */ - w368 = (w45*w367); + w368 = (w45 * w367); /* #1632: (@1[2] += @368) */ - for (rr=w1+2, ss=(&w368); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w368); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1633: @367 = (@331*@367) */ - w367 = (w331*w367); + w367 = (w331 * w367); /* #1634: (@1[1] += @367) */ - for (rr=w1+1, ss=(&w367); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w367); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1635: @366 = (@44*@366) */ - w366 = (w44*w366); + w366 = (w44 * w366); /* #1636: (@1[1] += @366) */ - for (rr=w1+1, ss=(&w366); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w366); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1637: @366 = (@43*@365) */ - w366 = (w43*w365); + w366 = (w43 * w365); /* #1638: (@1[17] += @366) */ - for (rr=w1+17, ss=(&w366); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w366); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1639: @365 = (@332*@365) */ - w365 = (w332*w365); + w365 = (w332 * w365); /* #1640: (@1[0] += @365) */ - for (rr=w1+0, ss=(&w365); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w365); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1641: @365 = (@42*@364) */ - w365 = (w42*w364); + w365 = (w42 * w364); /* #1642: (@1[16] += @365) */ - for (rr=w1+16, ss=(&w365); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w365); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1643: @364 = (@333*@364) */ - w364 = (w333*w364); + w364 = (w333 * w364); /* #1644: (@1[0] += @364) */ - for (rr=w1+0, ss=(&w364); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w364); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1645: @364 = (@41*@363) */ - w364 = (w41*w363); + w364 = (w41 * w363); /* #1646: (@1[15] += @364) */ - for (rr=w1+15, ss=(&w364); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w364); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1647: @363 = (@334*@363) */ - w363 = (w334*w363); + w363 = (w334 * w363); /* #1648: (@1[0] += @363) */ - for (rr=w1+0, ss=(&w363); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w363); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1649: @363 = (@40*@362) */ - w363 = (w40*w362); + w363 = (w40 * w362); /* #1650: (@1[14] += @363) */ - for (rr=w1+14, ss=(&w363); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w363); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1651: @362 = (@335*@362) */ - w362 = (w335*w362); + w362 = (w335 * w362); /* #1652: (@1[0] += @362) */ - for (rr=w1+0, ss=(&w362); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w362); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1653: @362 = (@39*@361) */ - w362 = (w39*w361); + w362 = (w39 * w361); /* #1654: (@1[13] += @362) */ - for (rr=w1+13, ss=(&w362); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w362); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1655: @361 = (@336*@361) */ - w361 = (w336*w361); + w361 = (w336 * w361); /* #1656: (@1[0] += @361) */ - for (rr=w1+0, ss=(&w361); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w361); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1657: @361 = (@38*@360) */ - w361 = (w38*w360); + w361 = (w38 * w360); /* #1658: (@1[12] += @361) */ - for (rr=w1+12, ss=(&w361); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w361); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1659: @360 = (@337*@360) */ - w360 = (w337*w360); + w360 = (w337 * w360); /* #1660: (@1[0] += @360) */ - for (rr=w1+0, ss=(&w360); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w360); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1661: @360 = (@37*@359) */ - w360 = (w37*w359); + w360 = (w37 * w359); /* #1662: (@1[11] += @360) */ - for (rr=w1+11, ss=(&w360); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w360); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1663: @359 = (@338*@359) */ - w359 = (w338*w359); + w359 = (w338 * w359); /* #1664: (@1[0] += @359) */ - for (rr=w1+0, ss=(&w359); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w359); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1665: @359 = (@36*@358) */ - w359 = (w36*w358); + w359 = (w36 * w358); /* #1666: (@1[10] += @359) */ - for (rr=w1+10, ss=(&w359); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w359); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1667: @358 = (@339*@358) */ - w358 = (w339*w358); + w358 = (w339 * w358); /* #1668: (@1[0] += @358) */ - for (rr=w1+0, ss=(&w358); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w358); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1669: @358 = (@35*@357) */ - w358 = (w35*w357); + w358 = (w35 * w357); /* #1670: (@1[9] += @358) */ - for (rr=w1+9, ss=(&w358); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w358); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1671: @357 = (@340*@357) */ - w357 = (w340*w357); + w357 = (w340 * w357); /* #1672: (@1[0] += @357) */ - for (rr=w1+0, ss=(&w357); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w357); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1673: @357 = (@34*@356) */ - w357 = (w34*w356); + w357 = (w34 * w356); /* #1674: (@1[8] += @357) */ - for (rr=w1+8, ss=(&w357); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w357); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1675: @356 = (@341*@356) */ - w356 = (w341*w356); + w356 = (w341 * w356); /* #1676: (@1[0] += @356) */ - for (rr=w1+0, ss=(&w356); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w356); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1677: @356 = (@33*@355) */ - w356 = (w33*w355); + w356 = (w33 * w355); /* #1678: (@1[7] += @356) */ - for (rr=w1+7, ss=(&w356); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w356); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1679: @355 = (@342*@355) */ - w355 = (w342*w355); + w355 = (w342 * w355); /* #1680: (@1[0] += @355) */ - for (rr=w1+0, ss=(&w355); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w355); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1681: @355 = (@32*@354) */ - w355 = (w32*w354); + w355 = (w32 * w354); /* #1682: (@1[6] += @355) */ - for (rr=w1+6, ss=(&w355); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w355); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1683: @354 = (@343*@354) */ - w354 = (w343*w354); + w354 = (w343 * w354); /* #1684: (@1[0] += @354) */ - for (rr=w1+0, ss=(&w354); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w354); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1685: @354 = (@31*@353) */ - w354 = (w31*w353); + w354 = (w31 * w353); /* #1686: (@1[5] += @354) */ - for (rr=w1+5, ss=(&w354); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w354); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1687: @353 = (@344*@353) */ - w353 = (w344*w353); + w353 = (w344 * w353); /* #1688: (@1[0] += @353) */ - for (rr=w1+0, ss=(&w353); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w353); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1689: @353 = (@30*@352) */ - w353 = (w30*w352); + w353 = (w30 * w352); /* #1690: (@1[4] += @353) */ - for (rr=w1+4, ss=(&w353); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w353); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1691: @352 = (@345*@352) */ - w352 = (w345*w352); + w352 = (w345 * w352); /* #1692: (@1[0] += @352) */ - for (rr=w1+0, ss=(&w352); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w352); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1693: @352 = (@29*@351) */ - w352 = (w29*w351); + w352 = (w29 * w351); /* #1694: (@1[3] += @352) */ - for (rr=w1+3, ss=(&w352); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w352); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1695: @351 = (@346*@351) */ - w351 = (w346*w351); + w351 = (w346 * w351); /* #1696: (@1[0] += @351) */ - for (rr=w1+0, ss=(&w351); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w351); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1697: @351 = (@28*@350) */ - w351 = (w28*w350); + w351 = (w28 * w350); /* #1698: (@1[2] += @351) */ - for (rr=w1+2, ss=(&w351); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w351); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1699: @350 = (@347*@350) */ - w350 = (w347*w350); + w350 = (w347 * w350); /* #1700: (@1[0] += @350) */ - for (rr=w1+0, ss=(&w350); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w350); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1701: @350 = (@27*@349) */ - w350 = (w27*w349); + w350 = (w27 * w349); /* #1702: (@1[1] += @350) */ - for (rr=w1+1, ss=(&w350); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w350); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1703: @349 = (@348*@349) */ - w349 = (w348*w349); + w349 = (w348 * w349); /* #1704: (@1[0] += @349) */ - for (rr=w1+0, ss=(&w349); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w349); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1705: @25 = (@26*@25) */ - w25 = (w26*w25); + w25 = (w26 * w25); /* #1706: (@1[0] += @25) */ - for (rr=w1+0, ss=(&w25); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w25); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1707: (@1[17] += @24) */ - for (rr=w1+17, ss=(&w24); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w24); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1708: (@1[16] += @23) */ - for (rr=w1+16, ss=(&w23); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w23); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1709: (@1[15] += @22) */ - for (rr=w1+15, ss=(&w22); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w22); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1710: (@1[14] += @21) */ - for (rr=w1+14, ss=(&w21); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w21); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1711: (@1[13] += @20) */ - for (rr=w1+13, ss=(&w20); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w20); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1712: (@1[12] += @19) */ - for (rr=w1+12, ss=(&w19); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w19); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1713: (@1[11] += @18) */ - for (rr=w1+11, ss=(&w18); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w18); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1714: (@1[10] += @17) */ - for (rr=w1+10, ss=(&w17); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w17); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1715: (@1[9] += @16) */ - for (rr=w1+9, ss=(&w16); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w16); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1716: (@1[8] += @15) */ - for (rr=w1+8, ss=(&w15); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w15); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1717: (@1[7] += @14) */ - for (rr=w1+7, ss=(&w14); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w14); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1718: (@1[6] += @13) */ - for (rr=w1+6, ss=(&w13); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w13); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #1719: (@1[5] += @12) */ - for (rr=w1+5, ss=(&w12); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w12); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #1720: (@1[4] += @11) */ - for (rr=w1+4, ss=(&w11); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w11); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #1721: (@1[3] += @10) */ - for (rr=w1+3, ss=(&w10); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w10); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #1722: (@1[2] += @9) */ - for (rr=w1+2, ss=(&w9); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w9); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #1723: (@1[1] += @8) */ - for (rr=w1+1, ss=(&w8); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w8); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #1724: (@1[0] += @7) */ - for (rr=w1+0, ss=(&w7); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w7); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #1725: @7 = @1[0] */ - for (rr=(&w7), ss=w1+0; ss!=w1+1; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 0; ss != w1 + 1; ss += 1) + *rr++ = *ss; /* #1726: (@0[18] = @7) */ - for (rr=w0+18, ss=(&w7); rr!=w0+19; rr+=1) *rr = *ss++; + for (rr = w0 + 18, ss = (&w7); rr != w0 + 19; rr += 1) + *rr = *ss++; /* #1727: @7 = @1[1] */ - for (rr=(&w7), ss=w1+1; ss!=w1+2; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 1; ss != w1 + 2; ss += 1) + *rr++ = *ss; /* #1728: (@0[19] = @7) */ - for (rr=w0+19, ss=(&w7); rr!=w0+20; rr+=1) *rr = *ss++; + for (rr = w0 + 19, ss = (&w7); rr != w0 + 20; rr += 1) + *rr = *ss++; /* #1729: @7 = @1[2] */ - for (rr=(&w7), ss=w1+2; ss!=w1+3; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 2; ss != w1 + 3; ss += 1) + *rr++ = *ss; /* #1730: (@0[20] = @7) */ - for (rr=w0+20, ss=(&w7); rr!=w0+21; rr+=1) *rr = *ss++; + for (rr = w0 + 20, ss = (&w7); rr != w0 + 21; rr += 1) + *rr = *ss++; /* #1731: @7 = @1[3] */ - for (rr=(&w7), ss=w1+3; ss!=w1+4; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 3; ss != w1 + 4; ss += 1) + *rr++ = *ss; /* #1732: (@0[21] = @7) */ - for (rr=w0+21, ss=(&w7); rr!=w0+22; rr+=1) *rr = *ss++; + for (rr = w0 + 21, ss = (&w7); rr != w0 + 22; rr += 1) + *rr = *ss++; /* #1733: @7 = @1[4] */ - for (rr=(&w7), ss=w1+4; ss!=w1+5; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 4; ss != w1 + 5; ss += 1) + *rr++ = *ss; /* #1734: (@0[22] = @7) */ - for (rr=w0+22, ss=(&w7); rr!=w0+23; rr+=1) *rr = *ss++; + for (rr = w0 + 22, ss = (&w7); rr != w0 + 23; rr += 1) + *rr = *ss++; /* #1735: @7 = @1[5] */ - for (rr=(&w7), ss=w1+5; ss!=w1+6; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 5; ss != w1 + 6; ss += 1) + *rr++ = *ss; /* #1736: (@0[23] = @7) */ - for (rr=w0+23, ss=(&w7); rr!=w0+24; rr+=1) *rr = *ss++; + for (rr = w0 + 23, ss = (&w7); rr != w0 + 24; rr += 1) + *rr = *ss++; /* #1737: @7 = @1[6] */ - for (rr=(&w7), ss=w1+6; ss!=w1+7; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 6; ss != w1 + 7; ss += 1) + *rr++ = *ss; /* #1738: (@0[24] = @7) */ - for (rr=w0+24, ss=(&w7); rr!=w0+25; rr+=1) *rr = *ss++; + for (rr = w0 + 24, ss = (&w7); rr != w0 + 25; rr += 1) + *rr = *ss++; /* #1739: @7 = @1[7] */ - for (rr=(&w7), ss=w1+7; ss!=w1+8; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 7; ss != w1 + 8; ss += 1) + *rr++ = *ss; /* #1740: (@0[25] = @7) */ - for (rr=w0+25, ss=(&w7); rr!=w0+26; rr+=1) *rr = *ss++; + for (rr = w0 + 25, ss = (&w7); rr != w0 + 26; rr += 1) + *rr = *ss++; /* #1741: @7 = @1[8] */ - for (rr=(&w7), ss=w1+8; ss!=w1+9; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 8; ss != w1 + 9; ss += 1) + *rr++ = *ss; /* #1742: (@0[26] = @7) */ - for (rr=w0+26, ss=(&w7); rr!=w0+27; rr+=1) *rr = *ss++; + for (rr = w0 + 26, ss = (&w7); rr != w0 + 27; rr += 1) + *rr = *ss++; /* #1743: @7 = @1[9] */ - for (rr=(&w7), ss=w1+9; ss!=w1+10; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 9; ss != w1 + 10; ss += 1) + *rr++ = *ss; /* #1744: (@0[27] = @7) */ - for (rr=w0+27, ss=(&w7); rr!=w0+28; rr+=1) *rr = *ss++; + for (rr = w0 + 27, ss = (&w7); rr != w0 + 28; rr += 1) + *rr = *ss++; /* #1745: @7 = @1[10] */ - for (rr=(&w7), ss=w1+10; ss!=w1+11; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 10; ss != w1 + 11; ss += 1) + *rr++ = *ss; /* #1746: (@0[28] = @7) */ - for (rr=w0+28, ss=(&w7); rr!=w0+29; rr+=1) *rr = *ss++; + for (rr = w0 + 28, ss = (&w7); rr != w0 + 29; rr += 1) + *rr = *ss++; /* #1747: @7 = @1[11] */ - for (rr=(&w7), ss=w1+11; ss!=w1+12; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 11; ss != w1 + 12; ss += 1) + *rr++ = *ss; /* #1748: (@0[29] = @7) */ - for (rr=w0+29, ss=(&w7); rr!=w0+30; rr+=1) *rr = *ss++; + for (rr = w0 + 29, ss = (&w7); rr != w0 + 30; rr += 1) + *rr = *ss++; /* #1749: @7 = @1[12] */ - for (rr=(&w7), ss=w1+12; ss!=w1+13; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 12; ss != w1 + 13; ss += 1) + *rr++ = *ss; /* #1750: (@0[30] = @7) */ - for (rr=w0+30, ss=(&w7); rr!=w0+31; rr+=1) *rr = *ss++; + for (rr = w0 + 30, ss = (&w7); rr != w0 + 31; rr += 1) + *rr = *ss++; /* #1751: @7 = @1[13] */ - for (rr=(&w7), ss=w1+13; ss!=w1+14; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 13; ss != w1 + 14; ss += 1) + *rr++ = *ss; /* #1752: (@0[31] = @7) */ - for (rr=w0+31, ss=(&w7); rr!=w0+32; rr+=1) *rr = *ss++; + for (rr = w0 + 31, ss = (&w7); rr != w0 + 32; rr += 1) + *rr = *ss++; /* #1753: @7 = @1[14] */ - for (rr=(&w7), ss=w1+14; ss!=w1+15; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 14; ss != w1 + 15; ss += 1) + *rr++ = *ss; /* #1754: (@0[32] = @7) */ - for (rr=w0+32, ss=(&w7); rr!=w0+33; rr+=1) *rr = *ss++; + for (rr = w0 + 32, ss = (&w7); rr != w0 + 33; rr += 1) + *rr = *ss++; /* #1755: @7 = @1[15] */ - for (rr=(&w7), ss=w1+15; ss!=w1+16; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 15; ss != w1 + 16; ss += 1) + *rr++ = *ss; /* #1756: (@0[33] = @7) */ - for (rr=w0+33, ss=(&w7); rr!=w0+34; rr+=1) *rr = *ss++; + for (rr = w0 + 33, ss = (&w7); rr != w0 + 34; rr += 1) + *rr = *ss++; /* #1757: @7 = @1[16] */ - for (rr=(&w7), ss=w1+16; ss!=w1+17; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 16; ss != w1 + 17; ss += 1) + *rr++ = *ss; /* #1758: (@0[34] = @7) */ - for (rr=w0+34, ss=(&w7); rr!=w0+35; rr+=1) *rr = *ss++; + for (rr = w0 + 34, ss = (&w7); rr != w0 + 35; rr += 1) + *rr = *ss++; /* #1759: @7 = @1[17] */ - for (rr=(&w7), ss=w1+17; ss!=w1+18; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 17; ss != w1 + 18; ss += 1) + *rr++ = *ss; /* #1760: (@0[35] = @7) */ - for (rr=w0+35, ss=(&w7); rr!=w0+36; rr+=1) *rr = *ss++; + for (rr = w0 + 35, ss = (&w7); rr != w0 + 36; rr += 1) + *rr = *ss++; /* #1761: @1 = zeros(18x1) */ casadi_clear(w1, 18); /* #1762: @4 = zeros(189x1) */ casadi_clear(w4, 189); /* #1763: @6 = @5' */ - for (i=0, rr=w6, cs=w5; i<189; ++i) for (j=0; j<3; ++j) rr[i+j*189] = *cs++; + for (i = 0, rr = w6, cs = w5; i < 189; ++i) + for (j = 0; j < 3; ++j) + rr[i + j * 189] = *cs++; /* #1764: @7 = ones(3x1,1nz) */ w7 = 1.; /* #1765: @4 = mac(@6,@7,@4) */ casadi_mtimes(w6, casadi_s2, (&w7), casadi_s6, w4, casadi_s4, w, 0); - /* #1766: {@7, @8, @9, @10, @11, @12, @13, @14, @15, @16, @17, @18, @19, @20, @21, @22, @23, @24, @25, @349, @350, @351, @352, @353, @354, @355, @356, @357, @358, @359, @360, @361, @362, @363, @364, @365, @366, @367, @368, @369, @370, @371, @372, @373, @374, @375, @376, @377, @378, @379, @380, @381, @382, @383, @384, @385, @386, @387, @388, @389, @390, @391, @392, @393, @394, @395, @396, @397, @398, @399, @400, @401, @402, @403, @404, @405, @406, @407, @408, @409, @410, @411, @412, @413, @414, @415, @416, @417, @418, @419, @420, @421, @422, @423, @424, @425, @426, @427, @428, @429, @430, @431, @432, @433, @434, @435, @436, @437, @438, @439, @440, @441, @442, @443, @444, @445, @446, @447, @448, @449, @450, @451, @452, @453, @454, @455, @456, @457, @458, @459, @460, @461, @462, @463, @464, @465, @466, @467, @468, @469, @470, @471, @472, @473, @474, @475, @476, @477, @478, @479, @480, @481, @482, @483, @484, @485, @486, @487, @488, @489, @490, @491, @492, @493, @494, @495, @496, @497, @498, @499, @500, @501, @502, @503, @504, @505, @506, @507, @508, @509, @510, @511, @512, @513, @514, @515, @516, @517, @518} = vertsplit(@4) */ + /* #1766: {@7, @8, @9, @10, @11, @12, @13, @14, @15, @16, @17, @18, @19, @20, + * @21, @22, @23, @24, @25, @349, @350, @351, @352, @353, @354, @355, @356, + * @357, @358, @359, @360, @361, @362, @363, @364, @365, @366, @367, @368, + * @369, @370, @371, @372, @373, @374, @375, @376, @377, @378, @379, @380, + * @381, @382, @383, @384, @385, @386, @387, @388, @389, @390, @391, @392, + * @393, @394, @395, @396, @397, @398, @399, @400, @401, @402, @403, @404, + * @405, @406, @407, @408, @409, @410, @411, @412, @413, @414, @415, @416, + * @417, @418, @419, @420, @421, @422, @423, @424, @425, @426, @427, @428, + * @429, @430, @431, @432, @433, @434, @435, @436, @437, @438, @439, @440, + * @441, @442, @443, @444, @445, @446, @447, @448, @449, @450, @451, @452, + * @453, @454, @455, @456, @457, @458, @459, @460, @461, @462, @463, @464, + * @465, @466, @467, @468, @469, @470, @471, @472, @473, @474, @475, @476, + * @477, @478, @479, @480, @481, @482, @483, @484, @485, @486, @487, @488, + * @489, @490, @491, @492, @493, @494, @495, @496, @497, @498, @499, @500, + * @501, @502, @503, @504, @505, @506, @507, @508, @509, @510, @511, @512, + * @513, @514, @515, @516, @517, @518} = vertsplit(@4) */ w7 = w4[0]; w8 = w4[1]; w9 = w4[2]; @@ -5554,1488 +7555,1888 @@ static int casadi_f1(const casadi_real** arg, casadi_real** res, casadi_int* iw, /* #1767: @3 = (@3*@518) */ w3 *= w518; /* #1768: (@1[17] += @3) */ - for (rr=w1+17, ss=(&w3); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w3); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1769: @195 = (@195*@517) */ w195 *= w517; /* #1770: (@1[17] += @195) */ - for (rr=w1+17, ss=(&w195); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w195); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1771: @196 = (@196*@517) */ w196 *= w517; /* #1772: (@1[16] += @196) */ - for (rr=w1+16, ss=(&w196); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w196); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1773: @194 = (@194*@516) */ w194 *= w516; /* #1774: (@1[16] += @194) */ - for (rr=w1+16, ss=(&w194); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w194); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1775: @193 = (@193*@515) */ w193 *= w515; /* #1776: (@1[17] += @193) */ - for (rr=w1+17, ss=(&w193); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w193); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1777: @197 = (@197*@515) */ w197 *= w515; /* #1778: (@1[15] += @197) */ - for (rr=w1+15, ss=(&w197); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w197); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1779: @192 = (@192*@514) */ w192 *= w514; /* #1780: (@1[16] += @192) */ - for (rr=w1+16, ss=(&w192); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w192); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1781: @198 = (@198*@514) */ w198 *= w514; /* #1782: (@1[15] += @198) */ - for (rr=w1+15, ss=(&w198); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w198); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1783: @191 = (@191*@513) */ w191 *= w513; /* #1784: (@1[15] += @191) */ - for (rr=w1+15, ss=(&w191); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w191); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1785: @190 = (@190*@512) */ w190 *= w512; /* #1786: (@1[17] += @190) */ - for (rr=w1+17, ss=(&w190); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w190); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1787: @199 = (@199*@512) */ w199 *= w512; /* #1788: (@1[14] += @199) */ - for (rr=w1+14, ss=(&w199); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w199); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1789: @189 = (@189*@511) */ w189 *= w511; /* #1790: (@1[16] += @189) */ - for (rr=w1+16, ss=(&w189); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w189); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1791: @200 = (@200*@511) */ w200 *= w511; /* #1792: (@1[14] += @200) */ - for (rr=w1+14, ss=(&w200); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w200); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1793: @188 = (@188*@510) */ w188 *= w510; /* #1794: (@1[15] += @188) */ - for (rr=w1+15, ss=(&w188); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w188); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1795: @201 = (@201*@510) */ w201 *= w510; /* #1796: (@1[14] += @201) */ - for (rr=w1+14, ss=(&w201); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w201); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1797: @187 = (@187*@509) */ w187 *= w509; /* #1798: (@1[14] += @187) */ - for (rr=w1+14, ss=(&w187); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w187); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1799: @186 = (@186*@508) */ w186 *= w508; /* #1800: (@1[17] += @186) */ - for (rr=w1+17, ss=(&w186); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w186); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1801: @202 = (@202*@508) */ w202 *= w508; /* #1802: (@1[13] += @202) */ - for (rr=w1+13, ss=(&w202); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w202); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1803: @185 = (@185*@507) */ w185 *= w507; /* #1804: (@1[16] += @185) */ - for (rr=w1+16, ss=(&w185); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w185); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1805: @203 = (@203*@507) */ w203 *= w507; /* #1806: (@1[13] += @203) */ - for (rr=w1+13, ss=(&w203); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w203); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1807: @184 = (@184*@506) */ w184 *= w506; /* #1808: (@1[15] += @184) */ - for (rr=w1+15, ss=(&w184); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w184); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1809: @204 = (@204*@506) */ w204 *= w506; /* #1810: (@1[13] += @204) */ - for (rr=w1+13, ss=(&w204); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w204); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1811: @183 = (@183*@505) */ w183 *= w505; /* #1812: (@1[14] += @183) */ - for (rr=w1+14, ss=(&w183); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w183); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1813: @205 = (@205*@505) */ w205 *= w505; /* #1814: (@1[13] += @205) */ - for (rr=w1+13, ss=(&w205); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w205); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1815: @182 = (@182*@504) */ w182 *= w504; /* #1816: (@1[13] += @182) */ - for (rr=w1+13, ss=(&w182); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w182); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1817: @181 = (@181*@503) */ w181 *= w503; /* #1818: (@1[17] += @181) */ - for (rr=w1+17, ss=(&w181); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w181); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1819: @206 = (@206*@503) */ w206 *= w503; /* #1820: (@1[12] += @206) */ - for (rr=w1+12, ss=(&w206); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w206); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1821: @180 = (@180*@502) */ w180 *= w502; /* #1822: (@1[16] += @180) */ - for (rr=w1+16, ss=(&w180); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w180); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1823: @207 = (@207*@502) */ w207 *= w502; /* #1824: (@1[12] += @207) */ - for (rr=w1+12, ss=(&w207); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w207); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1825: @179 = (@179*@501) */ w179 *= w501; /* #1826: (@1[15] += @179) */ - for (rr=w1+15, ss=(&w179); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w179); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1827: @208 = (@208*@501) */ w208 *= w501; /* #1828: (@1[12] += @208) */ - for (rr=w1+12, ss=(&w208); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w208); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1829: @178 = (@178*@500) */ w178 *= w500; /* #1830: (@1[14] += @178) */ - for (rr=w1+14, ss=(&w178); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w178); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1831: @209 = (@209*@500) */ w209 *= w500; /* #1832: (@1[12] += @209) */ - for (rr=w1+12, ss=(&w209); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w209); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1833: @177 = (@177*@499) */ w177 *= w499; /* #1834: (@1[13] += @177) */ - for (rr=w1+13, ss=(&w177); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w177); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1835: @210 = (@210*@499) */ w210 *= w499; /* #1836: (@1[12] += @210) */ - for (rr=w1+12, ss=(&w210); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w210); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1837: @176 = (@176*@498) */ w176 *= w498; /* #1838: (@1[12] += @176) */ - for (rr=w1+12, ss=(&w176); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w176); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1839: @175 = (@175*@497) */ w175 *= w497; /* #1840: (@1[17] += @175) */ - for (rr=w1+17, ss=(&w175); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w175); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1841: @211 = (@211*@497) */ w211 *= w497; /* #1842: (@1[11] += @211) */ - for (rr=w1+11, ss=(&w211); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w211); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1843: @174 = (@174*@496) */ w174 *= w496; /* #1844: (@1[16] += @174) */ - for (rr=w1+16, ss=(&w174); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w174); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1845: @212 = (@212*@496) */ w212 *= w496; /* #1846: (@1[11] += @212) */ - for (rr=w1+11, ss=(&w212); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w212); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1847: @173 = (@173*@495) */ w173 *= w495; /* #1848: (@1[15] += @173) */ - for (rr=w1+15, ss=(&w173); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w173); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1849: @213 = (@213*@495) */ w213 *= w495; /* #1850: (@1[11] += @213) */ - for (rr=w1+11, ss=(&w213); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w213); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1851: @172 = (@172*@494) */ w172 *= w494; /* #1852: (@1[14] += @172) */ - for (rr=w1+14, ss=(&w172); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w172); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1853: @214 = (@214*@494) */ w214 *= w494; /* #1854: (@1[11] += @214) */ - for (rr=w1+11, ss=(&w214); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w214); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1855: @171 = (@171*@493) */ w171 *= w493; /* #1856: (@1[13] += @171) */ - for (rr=w1+13, ss=(&w171); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w171); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1857: @215 = (@215*@493) */ w215 *= w493; /* #1858: (@1[11] += @215) */ - for (rr=w1+11, ss=(&w215); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w215); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1859: @170 = (@170*@492) */ w170 *= w492; /* #1860: (@1[12] += @170) */ - for (rr=w1+12, ss=(&w170); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w170); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1861: @216 = (@216*@492) */ w216 *= w492; /* #1862: (@1[11] += @216) */ - for (rr=w1+11, ss=(&w216); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w216); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1863: @169 = (@169*@491) */ w169 *= w491; /* #1864: (@1[11] += @169) */ - for (rr=w1+11, ss=(&w169); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w169); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1865: @168 = (@168*@490) */ w168 *= w490; /* #1866: (@1[17] += @168) */ - for (rr=w1+17, ss=(&w168); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w168); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1867: @217 = (@217*@490) */ w217 *= w490; /* #1868: (@1[10] += @217) */ - for (rr=w1+10, ss=(&w217); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w217); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1869: @167 = (@167*@489) */ w167 *= w489; /* #1870: (@1[16] += @167) */ - for (rr=w1+16, ss=(&w167); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w167); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1871: @218 = (@218*@489) */ w218 *= w489; /* #1872: (@1[10] += @218) */ - for (rr=w1+10, ss=(&w218); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w218); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1873: @166 = (@166*@488) */ w166 *= w488; /* #1874: (@1[15] += @166) */ - for (rr=w1+15, ss=(&w166); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w166); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1875: @219 = (@219*@488) */ w219 *= w488; /* #1876: (@1[10] += @219) */ - for (rr=w1+10, ss=(&w219); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w219); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1877: @165 = (@165*@487) */ w165 *= w487; /* #1878: (@1[14] += @165) */ - for (rr=w1+14, ss=(&w165); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w165); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1879: @220 = (@220*@487) */ w220 *= w487; /* #1880: (@1[10] += @220) */ - for (rr=w1+10, ss=(&w220); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w220); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1881: @164 = (@164*@486) */ w164 *= w486; /* #1882: (@1[13] += @164) */ - for (rr=w1+13, ss=(&w164); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w164); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1883: @221 = (@221*@486) */ w221 *= w486; /* #1884: (@1[10] += @221) */ - for (rr=w1+10, ss=(&w221); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w221); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1885: @163 = (@163*@485) */ w163 *= w485; /* #1886: (@1[12] += @163) */ - for (rr=w1+12, ss=(&w163); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w163); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1887: @222 = (@222*@485) */ w222 *= w485; /* #1888: (@1[10] += @222) */ - for (rr=w1+10, ss=(&w222); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w222); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1889: @162 = (@162*@484) */ w162 *= w484; /* #1890: (@1[11] += @162) */ - for (rr=w1+11, ss=(&w162); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w162); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1891: @223 = (@223*@484) */ w223 *= w484; /* #1892: (@1[10] += @223) */ - for (rr=w1+10, ss=(&w223); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w223); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1893: @161 = (@161*@483) */ w161 *= w483; /* #1894: (@1[10] += @161) */ - for (rr=w1+10, ss=(&w161); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w161); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1895: @160 = (@160*@482) */ w160 *= w482; /* #1896: (@1[17] += @160) */ - for (rr=w1+17, ss=(&w160); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w160); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1897: @224 = (@224*@482) */ w224 *= w482; /* #1898: (@1[9] += @224) */ - for (rr=w1+9, ss=(&w224); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w224); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1899: @159 = (@159*@481) */ w159 *= w481; /* #1900: (@1[16] += @159) */ - for (rr=w1+16, ss=(&w159); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w159); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1901: @225 = (@225*@481) */ w225 *= w481; /* #1902: (@1[9] += @225) */ - for (rr=w1+9, ss=(&w225); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w225); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1903: @158 = (@158*@480) */ w158 *= w480; /* #1904: (@1[15] += @158) */ - for (rr=w1+15, ss=(&w158); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w158); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1905: @226 = (@226*@480) */ w226 *= w480; /* #1906: (@1[9] += @226) */ - for (rr=w1+9, ss=(&w226); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w226); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1907: @157 = (@157*@479) */ w157 *= w479; /* #1908: (@1[14] += @157) */ - for (rr=w1+14, ss=(&w157); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w157); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1909: @227 = (@227*@479) */ w227 *= w479; /* #1910: (@1[9] += @227) */ - for (rr=w1+9, ss=(&w227); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w227); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1911: @156 = (@156*@478) */ w156 *= w478; /* #1912: (@1[13] += @156) */ - for (rr=w1+13, ss=(&w156); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w156); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1913: @228 = (@228*@478) */ w228 *= w478; /* #1914: (@1[9] += @228) */ - for (rr=w1+9, ss=(&w228); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w228); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1915: @155 = (@155*@477) */ w155 *= w477; /* #1916: (@1[12] += @155) */ - for (rr=w1+12, ss=(&w155); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w155); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1917: @229 = (@229*@477) */ w229 *= w477; /* #1918: (@1[9] += @229) */ - for (rr=w1+9, ss=(&w229); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w229); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1919: @154 = (@154*@476) */ w154 *= w476; /* #1920: (@1[11] += @154) */ - for (rr=w1+11, ss=(&w154); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w154); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1921: @230 = (@230*@476) */ w230 *= w476; /* #1922: (@1[9] += @230) */ - for (rr=w1+9, ss=(&w230); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w230); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1923: @153 = (@153*@475) */ w153 *= w475; /* #1924: (@1[10] += @153) */ - for (rr=w1+10, ss=(&w153); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w153); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1925: @231 = (@231*@475) */ w231 *= w475; /* #1926: (@1[9] += @231) */ - for (rr=w1+9, ss=(&w231); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w231); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1927: @152 = (@152*@474) */ w152 *= w474; /* #1928: (@1[9] += @152) */ - for (rr=w1+9, ss=(&w152); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w152); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1929: @151 = (@151*@473) */ w151 *= w473; /* #1930: (@1[17] += @151) */ - for (rr=w1+17, ss=(&w151); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w151); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1931: @232 = (@232*@473) */ w232 *= w473; /* #1932: (@1[8] += @232) */ - for (rr=w1+8, ss=(&w232); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w232); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1933: @150 = (@150*@472) */ w150 *= w472; /* #1934: (@1[16] += @150) */ - for (rr=w1+16, ss=(&w150); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w150); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1935: @233 = (@233*@472) */ w233 *= w472; /* #1936: (@1[8] += @233) */ - for (rr=w1+8, ss=(&w233); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w233); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1937: @149 = (@149*@471) */ w149 *= w471; /* #1938: (@1[15] += @149) */ - for (rr=w1+15, ss=(&w149); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w149); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1939: @234 = (@234*@471) */ w234 *= w471; /* #1940: (@1[8] += @234) */ - for (rr=w1+8, ss=(&w234); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w234); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1941: @148 = (@148*@470) */ w148 *= w470; /* #1942: (@1[14] += @148) */ - for (rr=w1+14, ss=(&w148); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w148); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1943: @235 = (@235*@470) */ w235 *= w470; /* #1944: (@1[8] += @235) */ - for (rr=w1+8, ss=(&w235); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w235); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1945: @147 = (@147*@469) */ w147 *= w469; /* #1946: (@1[13] += @147) */ - for (rr=w1+13, ss=(&w147); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w147); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1947: @236 = (@236*@469) */ w236 *= w469; /* #1948: (@1[8] += @236) */ - for (rr=w1+8, ss=(&w236); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w236); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1949: @146 = (@146*@468) */ w146 *= w468; /* #1950: (@1[12] += @146) */ - for (rr=w1+12, ss=(&w146); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w146); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1951: @237 = (@237*@468) */ w237 *= w468; /* #1952: (@1[8] += @237) */ - for (rr=w1+8, ss=(&w237); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w237); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1953: @145 = (@145*@467) */ w145 *= w467; /* #1954: (@1[11] += @145) */ - for (rr=w1+11, ss=(&w145); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w145); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1955: @238 = (@238*@467) */ w238 *= w467; /* #1956: (@1[8] += @238) */ - for (rr=w1+8, ss=(&w238); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w238); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1957: @144 = (@144*@466) */ w144 *= w466; /* #1958: (@1[10] += @144) */ - for (rr=w1+10, ss=(&w144); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w144); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1959: @239 = (@239*@466) */ w239 *= w466; /* #1960: (@1[8] += @239) */ - for (rr=w1+8, ss=(&w239); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w239); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1961: @143 = (@143*@465) */ w143 *= w465; /* #1962: (@1[9] += @143) */ - for (rr=w1+9, ss=(&w143); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w143); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #1963: @240 = (@240*@465) */ w240 *= w465; /* #1964: (@1[8] += @240) */ - for (rr=w1+8, ss=(&w240); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w240); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1965: @142 = (@142*@464) */ w142 *= w464; /* #1966: (@1[8] += @142) */ - for (rr=w1+8, ss=(&w142); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w142); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #1967: @141 = (@141*@463) */ w141 *= w463; /* #1968: (@1[17] += @141) */ - for (rr=w1+17, ss=(&w141); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w141); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #1969: @241 = (@241*@463) */ w241 *= w463; /* #1970: (@1[7] += @241) */ - for (rr=w1+7, ss=(&w241); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w241); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1971: @140 = (@140*@462) */ w140 *= w462; /* #1972: (@1[16] += @140) */ - for (rr=w1+16, ss=(&w140); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w140); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #1973: @242 = (@242*@462) */ w242 *= w462; /* #1974: (@1[7] += @242) */ - for (rr=w1+7, ss=(&w242); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w242); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1975: @139 = (@139*@461) */ w139 *= w461; /* #1976: (@1[15] += @139) */ - for (rr=w1+15, ss=(&w139); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w139); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #1977: @243 = (@243*@461) */ w243 *= w461; /* #1978: (@1[7] += @243) */ - for (rr=w1+7, ss=(&w243); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w243); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1979: @138 = (@138*@460) */ w138 *= w460; /* #1980: (@1[14] += @138) */ - for (rr=w1+14, ss=(&w138); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w138); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #1981: @244 = (@244*@460) */ w244 *= w460; /* #1982: (@1[7] += @244) */ - for (rr=w1+7, ss=(&w244); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w244); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1983: @137 = (@137*@459) */ w137 *= w459; /* #1984: (@1[13] += @137) */ - for (rr=w1+13, ss=(&w137); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w137); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #1985: @245 = (@245*@459) */ w245 *= w459; /* #1986: (@1[7] += @245) */ - for (rr=w1+7, ss=(&w245); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w245); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1987: @136 = (@136*@458) */ w136 *= w458; /* #1988: (@1[12] += @136) */ - for (rr=w1+12, ss=(&w136); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w136); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #1989: @246 = (@246*@458) */ w246 *= w458; /* #1990: (@1[7] += @246) */ - for (rr=w1+7, ss=(&w246); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w246); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1991: @135 = (@135*@457) */ w135 *= w457; /* #1992: (@1[11] += @135) */ - for (rr=w1+11, ss=(&w135); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w135); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #1993: @247 = (@247*@457) */ w247 *= w457; /* #1994: (@1[7] += @247) */ - for (rr=w1+7, ss=(&w247); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w247); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1995: @134 = (@134*@456) */ w134 *= w456; /* #1996: (@1[10] += @134) */ - for (rr=w1+10, ss=(&w134); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w134); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #1997: @248 = (@248*@456) */ w248 *= w456; /* #1998: (@1[7] += @248) */ - for (rr=w1+7, ss=(&w248); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w248); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #1999: @133 = (@133*@455) */ w133 *= w455; /* #2000: (@1[9] += @133) */ - for (rr=w1+9, ss=(&w133); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w133); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #2001: @249 = (@249*@455) */ w249 *= w455; /* #2002: (@1[7] += @249) */ - for (rr=w1+7, ss=(&w249); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w249); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2003: @132 = (@132*@454) */ w132 *= w454; /* #2004: (@1[8] += @132) */ - for (rr=w1+8, ss=(&w132); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w132); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #2005: @250 = (@250*@454) */ w250 *= w454; /* #2006: (@1[7] += @250) */ - for (rr=w1+7, ss=(&w250); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w250); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2007: @131 = (@131*@453) */ w131 *= w453; /* #2008: (@1[7] += @131) */ - for (rr=w1+7, ss=(&w131); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w131); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2009: @130 = (@130*@452) */ w130 *= w452; /* #2010: (@1[17] += @130) */ - for (rr=w1+17, ss=(&w130); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w130); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #2011: @251 = (@251*@452) */ w251 *= w452; /* #2012: (@1[6] += @251) */ - for (rr=w1+6, ss=(&w251); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w251); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2013: @129 = (@129*@451) */ w129 *= w451; /* #2014: (@1[16] += @129) */ - for (rr=w1+16, ss=(&w129); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w129); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #2015: @252 = (@252*@451) */ w252 *= w451; /* #2016: (@1[6] += @252) */ - for (rr=w1+6, ss=(&w252); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w252); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2017: @128 = (@128*@450) */ w128 *= w450; /* #2018: (@1[15] += @128) */ - for (rr=w1+15, ss=(&w128); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w128); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #2019: @253 = (@253*@450) */ w253 *= w450; /* #2020: (@1[6] += @253) */ - for (rr=w1+6, ss=(&w253); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w253); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2021: @127 = (@127*@449) */ w127 *= w449; /* #2022: (@1[14] += @127) */ - for (rr=w1+14, ss=(&w127); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w127); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #2023: @254 = (@254*@449) */ w254 *= w449; /* #2024: (@1[6] += @254) */ - for (rr=w1+6, ss=(&w254); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w254); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2025: @126 = (@126*@448) */ w126 *= w448; /* #2026: (@1[13] += @126) */ - for (rr=w1+13, ss=(&w126); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w126); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #2027: @255 = (@255*@448) */ w255 *= w448; /* #2028: (@1[6] += @255) */ - for (rr=w1+6, ss=(&w255); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w255); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2029: @125 = (@125*@447) */ w125 *= w447; /* #2030: (@1[12] += @125) */ - for (rr=w1+12, ss=(&w125); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w125); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #2031: @256 = (@256*@447) */ w256 *= w447; /* #2032: (@1[6] += @256) */ - for (rr=w1+6, ss=(&w256); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w256); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2033: @124 = (@124*@446) */ w124 *= w446; /* #2034: (@1[11] += @124) */ - for (rr=w1+11, ss=(&w124); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w124); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #2035: @257 = (@257*@446) */ w257 *= w446; /* #2036: (@1[6] += @257) */ - for (rr=w1+6, ss=(&w257); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w257); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2037: @123 = (@123*@445) */ w123 *= w445; /* #2038: (@1[10] += @123) */ - for (rr=w1+10, ss=(&w123); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w123); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #2039: @258 = (@258*@445) */ w258 *= w445; /* #2040: (@1[6] += @258) */ - for (rr=w1+6, ss=(&w258); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w258); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2041: @122 = (@122*@444) */ w122 *= w444; /* #2042: (@1[9] += @122) */ - for (rr=w1+9, ss=(&w122); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w122); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #2043: @259 = (@259*@444) */ w259 *= w444; /* #2044: (@1[6] += @259) */ - for (rr=w1+6, ss=(&w259); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w259); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2045: @121 = (@121*@443) */ w121 *= w443; /* #2046: (@1[8] += @121) */ - for (rr=w1+8, ss=(&w121); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w121); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #2047: @260 = (@260*@443) */ w260 *= w443; /* #2048: (@1[6] += @260) */ - for (rr=w1+6, ss=(&w260); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w260); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2049: @120 = (@120*@442) */ w120 *= w442; /* #2050: (@1[7] += @120) */ - for (rr=w1+7, ss=(&w120); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w120); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2051: @261 = (@261*@442) */ w261 *= w442; /* #2052: (@1[6] += @261) */ - for (rr=w1+6, ss=(&w261); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w261); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2053: @119 = (@119*@441) */ w119 *= w441; /* #2054: (@1[6] += @119) */ - for (rr=w1+6, ss=(&w119); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w119); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2055: @118 = (@118*@440) */ w118 *= w440; /* #2056: (@1[17] += @118) */ - for (rr=w1+17, ss=(&w118); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w118); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #2057: @262 = (@262*@440) */ w262 *= w440; /* #2058: (@1[5] += @262) */ - for (rr=w1+5, ss=(&w262); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w262); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2059: @117 = (@117*@439) */ w117 *= w439; /* #2060: (@1[16] += @117) */ - for (rr=w1+16, ss=(&w117); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w117); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #2061: @263 = (@263*@439) */ w263 *= w439; /* #2062: (@1[5] += @263) */ - for (rr=w1+5, ss=(&w263); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w263); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2063: @116 = (@116*@438) */ w116 *= w438; /* #2064: (@1[15] += @116) */ - for (rr=w1+15, ss=(&w116); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w116); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #2065: @264 = (@264*@438) */ w264 *= w438; /* #2066: (@1[5] += @264) */ - for (rr=w1+5, ss=(&w264); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w264); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2067: @115 = (@115*@437) */ w115 *= w437; /* #2068: (@1[14] += @115) */ - for (rr=w1+14, ss=(&w115); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w115); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #2069: @265 = (@265*@437) */ w265 *= w437; /* #2070: (@1[5] += @265) */ - for (rr=w1+5, ss=(&w265); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w265); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2071: @114 = (@114*@436) */ w114 *= w436; /* #2072: (@1[13] += @114) */ - for (rr=w1+13, ss=(&w114); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w114); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #2073: @266 = (@266*@436) */ w266 *= w436; /* #2074: (@1[5] += @266) */ - for (rr=w1+5, ss=(&w266); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w266); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2075: @113 = (@113*@435) */ w113 *= w435; /* #2076: (@1[12] += @113) */ - for (rr=w1+12, ss=(&w113); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w113); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #2077: @267 = (@267*@435) */ w267 *= w435; /* #2078: (@1[5] += @267) */ - for (rr=w1+5, ss=(&w267); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w267); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2079: @112 = (@112*@434) */ w112 *= w434; /* #2080: (@1[11] += @112) */ - for (rr=w1+11, ss=(&w112); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w112); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #2081: @268 = (@268*@434) */ w268 *= w434; /* #2082: (@1[5] += @268) */ - for (rr=w1+5, ss=(&w268); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w268); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2083: @111 = (@111*@433) */ w111 *= w433; /* #2084: (@1[10] += @111) */ - for (rr=w1+10, ss=(&w111); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w111); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #2085: @269 = (@269*@433) */ w269 *= w433; /* #2086: (@1[5] += @269) */ - for (rr=w1+5, ss=(&w269); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w269); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2087: @110 = (@110*@432) */ w110 *= w432; /* #2088: (@1[9] += @110) */ - for (rr=w1+9, ss=(&w110); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w110); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #2089: @270 = (@270*@432) */ w270 *= w432; /* #2090: (@1[5] += @270) */ - for (rr=w1+5, ss=(&w270); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w270); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2091: @109 = (@109*@431) */ w109 *= w431; /* #2092: (@1[8] += @109) */ - for (rr=w1+8, ss=(&w109); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w109); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #2093: @271 = (@271*@431) */ w271 *= w431; /* #2094: (@1[5] += @271) */ - for (rr=w1+5, ss=(&w271); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w271); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2095: @108 = (@108*@430) */ w108 *= w430; /* #2096: (@1[7] += @108) */ - for (rr=w1+7, ss=(&w108); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w108); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2097: @272 = (@272*@430) */ w272 *= w430; /* #2098: (@1[5] += @272) */ - for (rr=w1+5, ss=(&w272); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w272); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2099: @107 = (@107*@429) */ w107 *= w429; /* #2100: (@1[6] += @107) */ - for (rr=w1+6, ss=(&w107); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w107); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2101: @273 = (@273*@429) */ w273 *= w429; /* #2102: (@1[5] += @273) */ - for (rr=w1+5, ss=(&w273); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w273); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2103: @106 = (@106*@428) */ w106 *= w428; /* #2104: (@1[5] += @106) */ - for (rr=w1+5, ss=(&w106); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w106); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2105: @105 = (@105*@427) */ w105 *= w427; /* #2106: (@1[17] += @105) */ - for (rr=w1+17, ss=(&w105); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w105); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #2107: @274 = (@274*@427) */ w274 *= w427; /* #2108: (@1[4] += @274) */ - for (rr=w1+4, ss=(&w274); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w274); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2109: @104 = (@104*@426) */ w104 *= w426; /* #2110: (@1[16] += @104) */ - for (rr=w1+16, ss=(&w104); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w104); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #2111: @275 = (@275*@426) */ w275 *= w426; /* #2112: (@1[4] += @275) */ - for (rr=w1+4, ss=(&w275); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w275); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2113: @103 = (@103*@425) */ w103 *= w425; /* #2114: (@1[15] += @103) */ - for (rr=w1+15, ss=(&w103); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w103); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #2115: @276 = (@276*@425) */ w276 *= w425; /* #2116: (@1[4] += @276) */ - for (rr=w1+4, ss=(&w276); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w276); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2117: @102 = (@102*@424) */ w102 *= w424; /* #2118: (@1[14] += @102) */ - for (rr=w1+14, ss=(&w102); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w102); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #2119: @277 = (@277*@424) */ w277 *= w424; /* #2120: (@1[4] += @277) */ - for (rr=w1+4, ss=(&w277); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w277); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2121: @101 = (@101*@423) */ w101 *= w423; /* #2122: (@1[13] += @101) */ - for (rr=w1+13, ss=(&w101); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w101); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #2123: @278 = (@278*@423) */ w278 *= w423; /* #2124: (@1[4] += @278) */ - for (rr=w1+4, ss=(&w278); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w278); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2125: @100 = (@100*@422) */ w100 *= w422; /* #2126: (@1[12] += @100) */ - for (rr=w1+12, ss=(&w100); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w100); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #2127: @279 = (@279*@422) */ w279 *= w422; /* #2128: (@1[4] += @279) */ - for (rr=w1+4, ss=(&w279); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w279); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2129: @99 = (@99*@421) */ w99 *= w421; /* #2130: (@1[11] += @99) */ - for (rr=w1+11, ss=(&w99); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w99); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #2131: @280 = (@280*@421) */ w280 *= w421; /* #2132: (@1[4] += @280) */ - for (rr=w1+4, ss=(&w280); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w280); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2133: @98 = (@98*@420) */ w98 *= w420; /* #2134: (@1[10] += @98) */ - for (rr=w1+10, ss=(&w98); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w98); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #2135: @281 = (@281*@420) */ w281 *= w420; /* #2136: (@1[4] += @281) */ - for (rr=w1+4, ss=(&w281); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w281); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2137: @97 = (@97*@419) */ w97 *= w419; /* #2138: (@1[9] += @97) */ - for (rr=w1+9, ss=(&w97); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w97); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #2139: @282 = (@282*@419) */ w282 *= w419; /* #2140: (@1[4] += @282) */ - for (rr=w1+4, ss=(&w282); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w282); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2141: @96 = (@96*@418) */ w96 *= w418; /* #2142: (@1[8] += @96) */ - for (rr=w1+8, ss=(&w96); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w96); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #2143: @283 = (@283*@418) */ w283 *= w418; /* #2144: (@1[4] += @283) */ - for (rr=w1+4, ss=(&w283); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w283); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2145: @95 = (@95*@417) */ w95 *= w417; /* #2146: (@1[7] += @95) */ - for (rr=w1+7, ss=(&w95); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w95); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2147: @284 = (@284*@417) */ w284 *= w417; /* #2148: (@1[4] += @284) */ - for (rr=w1+4, ss=(&w284); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w284); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2149: @94 = (@94*@416) */ w94 *= w416; /* #2150: (@1[6] += @94) */ - for (rr=w1+6, ss=(&w94); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w94); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2151: @285 = (@285*@416) */ w285 *= w416; /* #2152: (@1[4] += @285) */ - for (rr=w1+4, ss=(&w285); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w285); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2153: @93 = (@93*@415) */ w93 *= w415; /* #2154: (@1[5] += @93) */ - for (rr=w1+5, ss=(&w93); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w93); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2155: @286 = (@286*@415) */ w286 *= w415; /* #2156: (@1[4] += @286) */ - for (rr=w1+4, ss=(&w286); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w286); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2157: @92 = (@92*@414) */ w92 *= w414; /* #2158: (@1[4] += @92) */ - for (rr=w1+4, ss=(&w92); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w92); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2159: @91 = (@91*@413) */ w91 *= w413; /* #2160: (@1[17] += @91) */ - for (rr=w1+17, ss=(&w91); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w91); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #2161: @287 = (@287*@413) */ w287 *= w413; /* #2162: (@1[3] += @287) */ - for (rr=w1+3, ss=(&w287); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w287); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2163: @90 = (@90*@412) */ w90 *= w412; /* #2164: (@1[16] += @90) */ - for (rr=w1+16, ss=(&w90); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w90); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #2165: @288 = (@288*@412) */ w288 *= w412; /* #2166: (@1[3] += @288) */ - for (rr=w1+3, ss=(&w288); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w288); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2167: @89 = (@89*@411) */ w89 *= w411; /* #2168: (@1[15] += @89) */ - for (rr=w1+15, ss=(&w89); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w89); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #2169: @289 = (@289*@411) */ w289 *= w411; /* #2170: (@1[3] += @289) */ - for (rr=w1+3, ss=(&w289); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w289); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2171: @88 = (@88*@410) */ w88 *= w410; /* #2172: (@1[14] += @88) */ - for (rr=w1+14, ss=(&w88); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w88); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #2173: @290 = (@290*@410) */ w290 *= w410; /* #2174: (@1[3] += @290) */ - for (rr=w1+3, ss=(&w290); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w290); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2175: @87 = (@87*@409) */ w87 *= w409; /* #2176: (@1[13] += @87) */ - for (rr=w1+13, ss=(&w87); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w87); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #2177: @291 = (@291*@409) */ w291 *= w409; /* #2178: (@1[3] += @291) */ - for (rr=w1+3, ss=(&w291); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w291); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2179: @86 = (@86*@408) */ w86 *= w408; /* #2180: (@1[12] += @86) */ - for (rr=w1+12, ss=(&w86); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w86); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #2181: @292 = (@292*@408) */ w292 *= w408; /* #2182: (@1[3] += @292) */ - for (rr=w1+3, ss=(&w292); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w292); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2183: @85 = (@85*@407) */ w85 *= w407; /* #2184: (@1[11] += @85) */ - for (rr=w1+11, ss=(&w85); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w85); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #2185: @293 = (@293*@407) */ w293 *= w407; /* #2186: (@1[3] += @293) */ - for (rr=w1+3, ss=(&w293); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w293); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2187: @84 = (@84*@406) */ w84 *= w406; /* #2188: (@1[10] += @84) */ - for (rr=w1+10, ss=(&w84); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w84); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #2189: @294 = (@294*@406) */ w294 *= w406; /* #2190: (@1[3] += @294) */ - for (rr=w1+3, ss=(&w294); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w294); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2191: @83 = (@83*@405) */ w83 *= w405; /* #2192: (@1[9] += @83) */ - for (rr=w1+9, ss=(&w83); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w83); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #2193: @295 = (@295*@405) */ w295 *= w405; /* #2194: (@1[3] += @295) */ - for (rr=w1+3, ss=(&w295); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w295); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2195: @82 = (@82*@404) */ w82 *= w404; /* #2196: (@1[8] += @82) */ - for (rr=w1+8, ss=(&w82); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w82); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #2197: @296 = (@296*@404) */ w296 *= w404; /* #2198: (@1[3] += @296) */ - for (rr=w1+3, ss=(&w296); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w296); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2199: @81 = (@81*@403) */ w81 *= w403; /* #2200: (@1[7] += @81) */ - for (rr=w1+7, ss=(&w81); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w81); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2201: @297 = (@297*@403) */ w297 *= w403; /* #2202: (@1[3] += @297) */ - for (rr=w1+3, ss=(&w297); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w297); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2203: @80 = (@80*@402) */ w80 *= w402; /* #2204: (@1[6] += @80) */ - for (rr=w1+6, ss=(&w80); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w80); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2205: @298 = (@298*@402) */ w298 *= w402; /* #2206: (@1[3] += @298) */ - for (rr=w1+3, ss=(&w298); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w298); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2207: @79 = (@79*@401) */ w79 *= w401; /* #2208: (@1[5] += @79) */ - for (rr=w1+5, ss=(&w79); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w79); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2209: @299 = (@299*@401) */ w299 *= w401; /* #2210: (@1[3] += @299) */ - for (rr=w1+3, ss=(&w299); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w299); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2211: @78 = (@78*@400) */ w78 *= w400; /* #2212: (@1[4] += @78) */ - for (rr=w1+4, ss=(&w78); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w78); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2213: @300 = (@300*@400) */ w300 *= w400; /* #2214: (@1[3] += @300) */ - for (rr=w1+3, ss=(&w300); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w300); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2215: @77 = (@77*@399) */ w77 *= w399; /* #2216: (@1[3] += @77) */ - for (rr=w1+3, ss=(&w77); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w77); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2217: @76 = (@76*@398) */ w76 *= w398; /* #2218: (@1[17] += @76) */ - for (rr=w1+17, ss=(&w76); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w76); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #2219: @301 = (@301*@398) */ w301 *= w398; /* #2220: (@1[2] += @301) */ - for (rr=w1+2, ss=(&w301); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w301); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2221: @75 = (@75*@397) */ w75 *= w397; /* #2222: (@1[16] += @75) */ - for (rr=w1+16, ss=(&w75); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w75); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #2223: @302 = (@302*@397) */ w302 *= w397; /* #2224: (@1[2] += @302) */ - for (rr=w1+2, ss=(&w302); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w302); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2225: @74 = (@74*@396) */ w74 *= w396; /* #2226: (@1[15] += @74) */ - for (rr=w1+15, ss=(&w74); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w74); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #2227: @303 = (@303*@396) */ w303 *= w396; /* #2228: (@1[2] += @303) */ - for (rr=w1+2, ss=(&w303); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w303); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2229: @73 = (@73*@395) */ w73 *= w395; /* #2230: (@1[14] += @73) */ - for (rr=w1+14, ss=(&w73); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w73); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #2231: @304 = (@304*@395) */ w304 *= w395; /* #2232: (@1[2] += @304) */ - for (rr=w1+2, ss=(&w304); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w304); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2233: @72 = (@72*@394) */ w72 *= w394; /* #2234: (@1[13] += @72) */ - for (rr=w1+13, ss=(&w72); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w72); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #2235: @305 = (@305*@394) */ w305 *= w394; /* #2236: (@1[2] += @305) */ - for (rr=w1+2, ss=(&w305); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w305); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2237: @71 = (@71*@393) */ w71 *= w393; /* #2238: (@1[12] += @71) */ - for (rr=w1+12, ss=(&w71); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w71); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #2239: @306 = (@306*@393) */ w306 *= w393; /* #2240: (@1[2] += @306) */ - for (rr=w1+2, ss=(&w306); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w306); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2241: @70 = (@70*@392) */ w70 *= w392; /* #2242: (@1[11] += @70) */ - for (rr=w1+11, ss=(&w70); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w70); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #2243: @307 = (@307*@392) */ w307 *= w392; /* #2244: (@1[2] += @307) */ - for (rr=w1+2, ss=(&w307); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w307); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2245: @69 = (@69*@391) */ w69 *= w391; /* #2246: (@1[10] += @69) */ - for (rr=w1+10, ss=(&w69); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w69); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #2247: @308 = (@308*@391) */ w308 *= w391; /* #2248: (@1[2] += @308) */ - for (rr=w1+2, ss=(&w308); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w308); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2249: @68 = (@68*@390) */ w68 *= w390; /* #2250: (@1[9] += @68) */ - for (rr=w1+9, ss=(&w68); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w68); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #2251: @309 = (@309*@390) */ w309 *= w390; /* #2252: (@1[2] += @309) */ - for (rr=w1+2, ss=(&w309); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w309); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2253: @67 = (@67*@389) */ w67 *= w389; /* #2254: (@1[8] += @67) */ - for (rr=w1+8, ss=(&w67); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w67); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #2255: @310 = (@310*@389) */ w310 *= w389; /* #2256: (@1[2] += @310) */ - for (rr=w1+2, ss=(&w310); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w310); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2257: @66 = (@66*@388) */ w66 *= w388; /* #2258: (@1[7] += @66) */ - for (rr=w1+7, ss=(&w66); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w66); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2259: @311 = (@311*@388) */ w311 *= w388; /* #2260: (@1[2] += @311) */ - for (rr=w1+2, ss=(&w311); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w311); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2261: @65 = (@65*@387) */ w65 *= w387; /* #2262: (@1[6] += @65) */ - for (rr=w1+6, ss=(&w65); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w65); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2263: @312 = (@312*@387) */ w312 *= w387; /* #2264: (@1[2] += @312) */ - for (rr=w1+2, ss=(&w312); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w312); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2265: @64 = (@64*@386) */ w64 *= w386; /* #2266: (@1[5] += @64) */ - for (rr=w1+5, ss=(&w64); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w64); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2267: @313 = (@313*@386) */ w313 *= w386; /* #2268: (@1[2] += @313) */ - for (rr=w1+2, ss=(&w313); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w313); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2269: @63 = (@63*@385) */ w63 *= w385; /* #2270: (@1[4] += @63) */ - for (rr=w1+4, ss=(&w63); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w63); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2271: @314 = (@314*@385) */ w314 *= w385; /* #2272: (@1[2] += @314) */ - for (rr=w1+2, ss=(&w314); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w314); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2273: @62 = (@62*@384) */ w62 *= w384; /* #2274: (@1[3] += @62) */ - for (rr=w1+3, ss=(&w62); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w62); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2275: @315 = (@315*@384) */ w315 *= w384; /* #2276: (@1[2] += @315) */ - for (rr=w1+2, ss=(&w315); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w315); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2277: @61 = (@61*@383) */ w61 *= w383; /* #2278: (@1[2] += @61) */ - for (rr=w1+2, ss=(&w61); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w61); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2279: @60 = (@60*@382) */ w60 *= w382; /* #2280: (@1[17] += @60) */ - for (rr=w1+17, ss=(&w60); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w60); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #2281: @316 = (@316*@382) */ w316 *= w382; /* #2282: (@1[1] += @316) */ - for (rr=w1+1, ss=(&w316); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w316); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2283: @59 = (@59*@381) */ w59 *= w381; /* #2284: (@1[16] += @59) */ - for (rr=w1+16, ss=(&w59); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w59); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #2285: @317 = (@317*@381) */ w317 *= w381; /* #2286: (@1[1] += @317) */ - for (rr=w1+1, ss=(&w317); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w317); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2287: @58 = (@58*@380) */ w58 *= w380; /* #2288: (@1[15] += @58) */ - for (rr=w1+15, ss=(&w58); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w58); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #2289: @318 = (@318*@380) */ w318 *= w380; /* #2290: (@1[1] += @318) */ - for (rr=w1+1, ss=(&w318); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w318); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2291: @57 = (@57*@379) */ w57 *= w379; /* #2292: (@1[14] += @57) */ - for (rr=w1+14, ss=(&w57); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w57); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #2293: @319 = (@319*@379) */ w319 *= w379; /* #2294: (@1[1] += @319) */ - for (rr=w1+1, ss=(&w319); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w319); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2295: @56 = (@56*@378) */ w56 *= w378; /* #2296: (@1[13] += @56) */ - for (rr=w1+13, ss=(&w56); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w56); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #2297: @320 = (@320*@378) */ w320 *= w378; /* #2298: (@1[1] += @320) */ - for (rr=w1+1, ss=(&w320); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w320); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2299: @55 = (@55*@377) */ w55 *= w377; /* #2300: (@1[12] += @55) */ - for (rr=w1+12, ss=(&w55); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w55); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #2301: @321 = (@321*@377) */ w321 *= w377; /* #2302: (@1[1] += @321) */ - for (rr=w1+1, ss=(&w321); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w321); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2303: @54 = (@54*@376) */ w54 *= w376; /* #2304: (@1[11] += @54) */ - for (rr=w1+11, ss=(&w54); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w54); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #2305: @322 = (@322*@376) */ w322 *= w376; /* #2306: (@1[1] += @322) */ - for (rr=w1+1, ss=(&w322); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w322); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2307: @53 = (@53*@375) */ w53 *= w375; /* #2308: (@1[10] += @53) */ - for (rr=w1+10, ss=(&w53); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w53); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #2309: @323 = (@323*@375) */ w323 *= w375; /* #2310: (@1[1] += @323) */ - for (rr=w1+1, ss=(&w323); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w323); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2311: @52 = (@52*@374) */ w52 *= w374; /* #2312: (@1[9] += @52) */ - for (rr=w1+9, ss=(&w52); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w52); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #2313: @324 = (@324*@374) */ w324 *= w374; /* #2314: (@1[1] += @324) */ - for (rr=w1+1, ss=(&w324); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w324); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2315: @51 = (@51*@373) */ w51 *= w373; /* #2316: (@1[8] += @51) */ - for (rr=w1+8, ss=(&w51); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w51); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #2317: @325 = (@325*@373) */ w325 *= w373; /* #2318: (@1[1] += @325) */ - for (rr=w1+1, ss=(&w325); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w325); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2319: @50 = (@50*@372) */ w50 *= w372; /* #2320: (@1[7] += @50) */ - for (rr=w1+7, ss=(&w50); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w50); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2321: @326 = (@326*@372) */ w326 *= w372; /* #2322: (@1[1] += @326) */ - for (rr=w1+1, ss=(&w326); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w326); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2323: @49 = (@49*@371) */ w49 *= w371; /* #2324: (@1[6] += @49) */ - for (rr=w1+6, ss=(&w49); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w49); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2325: @327 = (@327*@371) */ w327 *= w371; /* #2326: (@1[1] += @327) */ - for (rr=w1+1, ss=(&w327); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w327); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2327: @48 = (@48*@370) */ w48 *= w370; /* #2328: (@1[5] += @48) */ - for (rr=w1+5, ss=(&w48); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w48); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2329: @328 = (@328*@370) */ w328 *= w370; /* #2330: (@1[1] += @328) */ - for (rr=w1+1, ss=(&w328); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w328); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2331: @47 = (@47*@369) */ w47 *= w369; /* #2332: (@1[4] += @47) */ - for (rr=w1+4, ss=(&w47); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w47); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2333: @329 = (@329*@369) */ w329 *= w369; /* #2334: (@1[1] += @329) */ - for (rr=w1+1, ss=(&w329); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w329); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2335: @46 = (@46*@368) */ w46 *= w368; /* #2336: (@1[3] += @46) */ - for (rr=w1+3, ss=(&w46); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w46); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2337: @330 = (@330*@368) */ w330 *= w368; /* #2338: (@1[1] += @330) */ - for (rr=w1+1, ss=(&w330); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w330); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2339: @45 = (@45*@367) */ w45 *= w367; /* #2340: (@1[2] += @45) */ - for (rr=w1+2, ss=(&w45); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w45); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2341: @331 = (@331*@367) */ w331 *= w367; /* #2342: (@1[1] += @331) */ - for (rr=w1+1, ss=(&w331); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w331); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2343: @44 = (@44*@366) */ w44 *= w366; /* #2344: (@1[1] += @44) */ - for (rr=w1+1, ss=(&w44); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w44); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2345: @43 = (@43*@365) */ w43 *= w365; /* #2346: (@1[17] += @43) */ - for (rr=w1+17, ss=(&w43); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w43); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #2347: @332 = (@332*@365) */ w332 *= w365; /* #2348: (@1[0] += @332) */ - for (rr=w1+0, ss=(&w332); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w332); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2349: @42 = (@42*@364) */ w42 *= w364; /* #2350: (@1[16] += @42) */ - for (rr=w1+16, ss=(&w42); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w42); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #2351: @333 = (@333*@364) */ w333 *= w364; /* #2352: (@1[0] += @333) */ - for (rr=w1+0, ss=(&w333); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w333); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2353: @41 = (@41*@363) */ w41 *= w363; /* #2354: (@1[15] += @41) */ - for (rr=w1+15, ss=(&w41); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w41); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #2355: @334 = (@334*@363) */ w334 *= w363; /* #2356: (@1[0] += @334) */ - for (rr=w1+0, ss=(&w334); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w334); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2357: @40 = (@40*@362) */ w40 *= w362; /* #2358: (@1[14] += @40) */ - for (rr=w1+14, ss=(&w40); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w40); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #2359: @335 = (@335*@362) */ w335 *= w362; /* #2360: (@1[0] += @335) */ - for (rr=w1+0, ss=(&w335); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w335); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2361: @39 = (@39*@361) */ w39 *= w361; /* #2362: (@1[13] += @39) */ - for (rr=w1+13, ss=(&w39); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w39); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #2363: @336 = (@336*@361) */ w336 *= w361; /* #2364: (@1[0] += @336) */ - for (rr=w1+0, ss=(&w336); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w336); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2365: @38 = (@38*@360) */ w38 *= w360; /* #2366: (@1[12] += @38) */ - for (rr=w1+12, ss=(&w38); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w38); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #2367: @337 = (@337*@360) */ w337 *= w360; /* #2368: (@1[0] += @337) */ - for (rr=w1+0, ss=(&w337); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w337); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2369: @37 = (@37*@359) */ w37 *= w359; /* #2370: (@1[11] += @37) */ - for (rr=w1+11, ss=(&w37); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w37); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #2371: @338 = (@338*@359) */ w338 *= w359; /* #2372: (@1[0] += @338) */ - for (rr=w1+0, ss=(&w338); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w338); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2373: @36 = (@36*@358) */ w36 *= w358; /* #2374: (@1[10] += @36) */ - for (rr=w1+10, ss=(&w36); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w36); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #2375: @339 = (@339*@358) */ w339 *= w358; /* #2376: (@1[0] += @339) */ - for (rr=w1+0, ss=(&w339); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w339); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2377: @35 = (@35*@357) */ w35 *= w357; /* #2378: (@1[9] += @35) */ - for (rr=w1+9, ss=(&w35); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w35); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #2379: @340 = (@340*@357) */ w340 *= w357; /* #2380: (@1[0] += @340) */ - for (rr=w1+0, ss=(&w340); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w340); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2381: @34 = (@34*@356) */ w34 *= w356; /* #2382: (@1[8] += @34) */ - for (rr=w1+8, ss=(&w34); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w34); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #2383: @341 = (@341*@356) */ w341 *= w356; /* #2384: (@1[0] += @341) */ - for (rr=w1+0, ss=(&w341); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w341); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2385: @33 = (@33*@355) */ w33 *= w355; /* #2386: (@1[7] += @33) */ - for (rr=w1+7, ss=(&w33); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w33); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2387: @342 = (@342*@355) */ w342 *= w355; /* #2388: (@1[0] += @342) */ - for (rr=w1+0, ss=(&w342); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w342); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2389: @32 = (@32*@354) */ w32 *= w354; /* #2390: (@1[6] += @32) */ - for (rr=w1+6, ss=(&w32); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w32); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2391: @343 = (@343*@354) */ w343 *= w354; /* #2392: (@1[0] += @343) */ - for (rr=w1+0, ss=(&w343); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w343); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2393: @31 = (@31*@353) */ w31 *= w353; /* #2394: (@1[5] += @31) */ - for (rr=w1+5, ss=(&w31); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w31); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2395: @344 = (@344*@353) */ w344 *= w353; /* #2396: (@1[0] += @344) */ - for (rr=w1+0, ss=(&w344); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w344); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2397: @30 = (@30*@352) */ w30 *= w352; /* #2398: (@1[4] += @30) */ - for (rr=w1+4, ss=(&w30); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w30); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2399: @345 = (@345*@352) */ w345 *= w352; /* #2400: (@1[0] += @345) */ - for (rr=w1+0, ss=(&w345); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w345); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2401: @29 = (@29*@351) */ w29 *= w351; /* #2402: (@1[3] += @29) */ - for (rr=w1+3, ss=(&w29); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w29); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2403: @346 = (@346*@351) */ w346 *= w351; /* #2404: (@1[0] += @346) */ - for (rr=w1+0, ss=(&w346); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w346); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2405: @28 = (@28*@350) */ w28 *= w350; /* #2406: (@1[2] += @28) */ - for (rr=w1+2, ss=(&w28); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w28); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2407: @347 = (@347*@350) */ w347 *= w350; /* #2408: (@1[0] += @347) */ - for (rr=w1+0, ss=(&w347); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w347); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2409: @27 = (@27*@349) */ w27 *= w349; /* #2410: (@1[1] += @27) */ - for (rr=w1+1, ss=(&w27); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w27); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2411: @348 = (@348*@349) */ w348 *= w349; /* #2412: (@1[0] += @348) */ - for (rr=w1+0, ss=(&w348); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w348); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2413: @26 = (@26*@25) */ w26 *= w25; /* #2414: (@1[0] += @26) */ - for (rr=w1+0, ss=(&w26); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w26); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2415: (@1[17] += @24) */ - for (rr=w1+17, ss=(&w24); rr!=w1+18; rr+=1) *rr += *ss++; + for (rr = w1 + 17, ss = (&w24); rr != w1 + 18; rr += 1) + *rr += *ss++; /* #2416: (@1[16] += @23) */ - for (rr=w1+16, ss=(&w23); rr!=w1+17; rr+=1) *rr += *ss++; + for (rr = w1 + 16, ss = (&w23); rr != w1 + 17; rr += 1) + *rr += *ss++; /* #2417: (@1[15] += @22) */ - for (rr=w1+15, ss=(&w22); rr!=w1+16; rr+=1) *rr += *ss++; + for (rr = w1 + 15, ss = (&w22); rr != w1 + 16; rr += 1) + *rr += *ss++; /* #2418: (@1[14] += @21) */ - for (rr=w1+14, ss=(&w21); rr!=w1+15; rr+=1) *rr += *ss++; + for (rr = w1 + 14, ss = (&w21); rr != w1 + 15; rr += 1) + *rr += *ss++; /* #2419: (@1[13] += @20) */ - for (rr=w1+13, ss=(&w20); rr!=w1+14; rr+=1) *rr += *ss++; + for (rr = w1 + 13, ss = (&w20); rr != w1 + 14; rr += 1) + *rr += *ss++; /* #2420: (@1[12] += @19) */ - for (rr=w1+12, ss=(&w19); rr!=w1+13; rr+=1) *rr += *ss++; + for (rr = w1 + 12, ss = (&w19); rr != w1 + 13; rr += 1) + *rr += *ss++; /* #2421: (@1[11] += @18) */ - for (rr=w1+11, ss=(&w18); rr!=w1+12; rr+=1) *rr += *ss++; + for (rr = w1 + 11, ss = (&w18); rr != w1 + 12; rr += 1) + *rr += *ss++; /* #2422: (@1[10] += @17) */ - for (rr=w1+10, ss=(&w17); rr!=w1+11; rr+=1) *rr += *ss++; + for (rr = w1 + 10, ss = (&w17); rr != w1 + 11; rr += 1) + *rr += *ss++; /* #2423: (@1[9] += @16) */ - for (rr=w1+9, ss=(&w16); rr!=w1+10; rr+=1) *rr += *ss++; + for (rr = w1 + 9, ss = (&w16); rr != w1 + 10; rr += 1) + *rr += *ss++; /* #2424: (@1[8] += @15) */ - for (rr=w1+8, ss=(&w15); rr!=w1+9; rr+=1) *rr += *ss++; + for (rr = w1 + 8, ss = (&w15); rr != w1 + 9; rr += 1) + *rr += *ss++; /* #2425: (@1[7] += @14) */ - for (rr=w1+7, ss=(&w14); rr!=w1+8; rr+=1) *rr += *ss++; + for (rr = w1 + 7, ss = (&w14); rr != w1 + 8; rr += 1) + *rr += *ss++; /* #2426: (@1[6] += @13) */ - for (rr=w1+6, ss=(&w13); rr!=w1+7; rr+=1) *rr += *ss++; + for (rr = w1 + 6, ss = (&w13); rr != w1 + 7; rr += 1) + *rr += *ss++; /* #2427: (@1[5] += @12) */ - for (rr=w1+5, ss=(&w12); rr!=w1+6; rr+=1) *rr += *ss++; + for (rr = w1 + 5, ss = (&w12); rr != w1 + 6; rr += 1) + *rr += *ss++; /* #2428: (@1[4] += @11) */ - for (rr=w1+4, ss=(&w11); rr!=w1+5; rr+=1) *rr += *ss++; + for (rr = w1 + 4, ss = (&w11); rr != w1 + 5; rr += 1) + *rr += *ss++; /* #2429: (@1[3] += @10) */ - for (rr=w1+3, ss=(&w10); rr!=w1+4; rr+=1) *rr += *ss++; + for (rr = w1 + 3, ss = (&w10); rr != w1 + 4; rr += 1) + *rr += *ss++; /* #2430: (@1[2] += @9) */ - for (rr=w1+2, ss=(&w9); rr!=w1+3; rr+=1) *rr += *ss++; + for (rr = w1 + 2, ss = (&w9); rr != w1 + 3; rr += 1) + *rr += *ss++; /* #2431: (@1[1] += @8) */ - for (rr=w1+1, ss=(&w8); rr!=w1+2; rr+=1) *rr += *ss++; + for (rr = w1 + 1, ss = (&w8); rr != w1 + 2; rr += 1) + *rr += *ss++; /* #2432: (@1[0] += @7) */ - for (rr=w1+0, ss=(&w7); rr!=w1+1; rr+=1) *rr += *ss++; + for (rr = w1 + 0, ss = (&w7); rr != w1 + 1; rr += 1) + *rr += *ss++; /* #2433: @7 = @1[0] */ - for (rr=(&w7), ss=w1+0; ss!=w1+1; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 0; ss != w1 + 1; ss += 1) + *rr++ = *ss; /* #2434: (@0[36] = @7) */ - for (rr=w0+36, ss=(&w7); rr!=w0+37; rr+=1) *rr = *ss++; + for (rr = w0 + 36, ss = (&w7); rr != w0 + 37; rr += 1) + *rr = *ss++; /* #2435: @7 = @1[1] */ - for (rr=(&w7), ss=w1+1; ss!=w1+2; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 1; ss != w1 + 2; ss += 1) + *rr++ = *ss; /* #2436: (@0[37] = @7) */ - for (rr=w0+37, ss=(&w7); rr!=w0+38; rr+=1) *rr = *ss++; + for (rr = w0 + 37, ss = (&w7); rr != w0 + 38; rr += 1) + *rr = *ss++; /* #2437: @7 = @1[2] */ - for (rr=(&w7), ss=w1+2; ss!=w1+3; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 2; ss != w1 + 3; ss += 1) + *rr++ = *ss; /* #2438: (@0[38] = @7) */ - for (rr=w0+38, ss=(&w7); rr!=w0+39; rr+=1) *rr = *ss++; + for (rr = w0 + 38, ss = (&w7); rr != w0 + 39; rr += 1) + *rr = *ss++; /* #2439: @7 = @1[3] */ - for (rr=(&w7), ss=w1+3; ss!=w1+4; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 3; ss != w1 + 4; ss += 1) + *rr++ = *ss; /* #2440: (@0[39] = @7) */ - for (rr=w0+39, ss=(&w7); rr!=w0+40; rr+=1) *rr = *ss++; + for (rr = w0 + 39, ss = (&w7); rr != w0 + 40; rr += 1) + *rr = *ss++; /* #2441: @7 = @1[4] */ - for (rr=(&w7), ss=w1+4; ss!=w1+5; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 4; ss != w1 + 5; ss += 1) + *rr++ = *ss; /* #2442: (@0[40] = @7) */ - for (rr=w0+40, ss=(&w7); rr!=w0+41; rr+=1) *rr = *ss++; + for (rr = w0 + 40, ss = (&w7); rr != w0 + 41; rr += 1) + *rr = *ss++; /* #2443: @7 = @1[5] */ - for (rr=(&w7), ss=w1+5; ss!=w1+6; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 5; ss != w1 + 6; ss += 1) + *rr++ = *ss; /* #2444: (@0[41] = @7) */ - for (rr=w0+41, ss=(&w7); rr!=w0+42; rr+=1) *rr = *ss++; + for (rr = w0 + 41, ss = (&w7); rr != w0 + 42; rr += 1) + *rr = *ss++; /* #2445: @7 = @1[6] */ - for (rr=(&w7), ss=w1+6; ss!=w1+7; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 6; ss != w1 + 7; ss += 1) + *rr++ = *ss; /* #2446: (@0[42] = @7) */ - for (rr=w0+42, ss=(&w7); rr!=w0+43; rr+=1) *rr = *ss++; + for (rr = w0 + 42, ss = (&w7); rr != w0 + 43; rr += 1) + *rr = *ss++; /* #2447: @7 = @1[7] */ - for (rr=(&w7), ss=w1+7; ss!=w1+8; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 7; ss != w1 + 8; ss += 1) + *rr++ = *ss; /* #2448: (@0[43] = @7) */ - for (rr=w0+43, ss=(&w7); rr!=w0+44; rr+=1) *rr = *ss++; + for (rr = w0 + 43, ss = (&w7); rr != w0 + 44; rr += 1) + *rr = *ss++; /* #2449: @7 = @1[8] */ - for (rr=(&w7), ss=w1+8; ss!=w1+9; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 8; ss != w1 + 9; ss += 1) + *rr++ = *ss; /* #2450: (@0[44] = @7) */ - for (rr=w0+44, ss=(&w7); rr!=w0+45; rr+=1) *rr = *ss++; + for (rr = w0 + 44, ss = (&w7); rr != w0 + 45; rr += 1) + *rr = *ss++; /* #2451: @7 = @1[9] */ - for (rr=(&w7), ss=w1+9; ss!=w1+10; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 9; ss != w1 + 10; ss += 1) + *rr++ = *ss; /* #2452: (@0[45] = @7) */ - for (rr=w0+45, ss=(&w7); rr!=w0+46; rr+=1) *rr = *ss++; + for (rr = w0 + 45, ss = (&w7); rr != w0 + 46; rr += 1) + *rr = *ss++; /* #2453: @7 = @1[10] */ - for (rr=(&w7), ss=w1+10; ss!=w1+11; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 10; ss != w1 + 11; ss += 1) + *rr++ = *ss; /* #2454: (@0[46] = @7) */ - for (rr=w0+46, ss=(&w7); rr!=w0+47; rr+=1) *rr = *ss++; + for (rr = w0 + 46, ss = (&w7); rr != w0 + 47; rr += 1) + *rr = *ss++; /* #2455: @7 = @1[11] */ - for (rr=(&w7), ss=w1+11; ss!=w1+12; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 11; ss != w1 + 12; ss += 1) + *rr++ = *ss; /* #2456: (@0[47] = @7) */ - for (rr=w0+47, ss=(&w7); rr!=w0+48; rr+=1) *rr = *ss++; + for (rr = w0 + 47, ss = (&w7); rr != w0 + 48; rr += 1) + *rr = *ss++; /* #2457: @7 = @1[12] */ - for (rr=(&w7), ss=w1+12; ss!=w1+13; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 12; ss != w1 + 13; ss += 1) + *rr++ = *ss; /* #2458: (@0[48] = @7) */ - for (rr=w0+48, ss=(&w7); rr!=w0+49; rr+=1) *rr = *ss++; + for (rr = w0 + 48, ss = (&w7); rr != w0 + 49; rr += 1) + *rr = *ss++; /* #2459: @7 = @1[13] */ - for (rr=(&w7), ss=w1+13; ss!=w1+14; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 13; ss != w1 + 14; ss += 1) + *rr++ = *ss; /* #2460: (@0[49] = @7) */ - for (rr=w0+49, ss=(&w7); rr!=w0+50; rr+=1) *rr = *ss++; + for (rr = w0 + 49, ss = (&w7); rr != w0 + 50; rr += 1) + *rr = *ss++; /* #2461: @7 = @1[14] */ - for (rr=(&w7), ss=w1+14; ss!=w1+15; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 14; ss != w1 + 15; ss += 1) + *rr++ = *ss; /* #2462: (@0[50] = @7) */ - for (rr=w0+50, ss=(&w7); rr!=w0+51; rr+=1) *rr = *ss++; + for (rr = w0 + 50, ss = (&w7); rr != w0 + 51; rr += 1) + *rr = *ss++; /* #2463: @7 = @1[15] */ - for (rr=(&w7), ss=w1+15; ss!=w1+16; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 15; ss != w1 + 16; ss += 1) + *rr++ = *ss; /* #2464: (@0[51] = @7) */ - for (rr=w0+51, ss=(&w7); rr!=w0+52; rr+=1) *rr = *ss++; + for (rr = w0 + 51, ss = (&w7); rr != w0 + 52; rr += 1) + *rr = *ss++; /* #2465: @7 = @1[16] */ - for (rr=(&w7), ss=w1+16; ss!=w1+17; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 16; ss != w1 + 17; ss += 1) + *rr++ = *ss; /* #2466: (@0[52] = @7) */ - for (rr=w0+52, ss=(&w7); rr!=w0+53; rr+=1) *rr = *ss++; + for (rr = w0 + 52, ss = (&w7); rr != w0 + 53; rr += 1) + *rr = *ss++; /* #2467: @7 = @1[17] */ - for (rr=(&w7), ss=w1+17; ss!=w1+18; ss+=1) *rr++ = *ss; + for (rr = (&w7), ss = w1 + 17; ss != w1 + 18; ss += 1) + *rr++ = *ss; /* #2468: (@0[53] = @7) */ - for (rr=w0+53, ss=(&w7); rr!=w0+54; rr+=1) *rr = *ss++; + for (rr = w0 + 53, ss = (&w7); rr != w0 + 54; rr += 1) + *rr = *ss++; /* #2469: @519 = @0' */ - for (i=0, rr=w519, cs=w0; i<3; ++i) for (j=0; j<18; ++j) rr[i+j*3] = *cs++; + for (i = 0, rr = w519, cs = w0; i < 3; ++i) + for (j = 0; j < 18; ++j) + rr[i + j * 3] = *cs++; /* #2470: output[0][0] = @519 */ casadi_copy(w519, 54, res[0]); return 0; } -extern "C" CASADI_SYMBOL_EXPORT int jac_Q_xyz_func(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ +extern "C" CASADI_SYMBOL_EXPORT int jac_Q_xyz_func(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 jac_Q_xyz_func_alloc_mem(void) { - return 0; -} +extern "C" CASADI_SYMBOL_EXPORT int jac_Q_xyz_func_alloc_mem(void) { return 0; } extern "C" CASADI_SYMBOL_EXPORT int jac_Q_xyz_func_init_mem(int mem) { return 0; } -extern "C" CASADI_SYMBOL_EXPORT void jac_Q_xyz_func_free_mem(int mem) { -} +extern "C" CASADI_SYMBOL_EXPORT void jac_Q_xyz_func_free_mem(int mem) {} -extern "C" CASADI_SYMBOL_EXPORT int jac_Q_xyz_func_checkout(void) { - return 0; -} +extern "C" CASADI_SYMBOL_EXPORT int jac_Q_xyz_func_checkout(void) { return 0; } -extern "C" CASADI_SYMBOL_EXPORT void jac_Q_xyz_func_release(int mem) { -} +extern "C" CASADI_SYMBOL_EXPORT void jac_Q_xyz_func_release(int mem) {} -extern "C" CASADI_SYMBOL_EXPORT void jac_Q_xyz_func_incref(void) { -} +extern "C" CASADI_SYMBOL_EXPORT void jac_Q_xyz_func_incref(void) {} -extern "C" CASADI_SYMBOL_EXPORT void jac_Q_xyz_func_decref(void) { -} +extern "C" CASADI_SYMBOL_EXPORT void jac_Q_xyz_func_decref(void) {} -extern "C" CASADI_SYMBOL_EXPORT casadi_int jac_Q_xyz_func_n_in(void) { return 2;} +extern "C" CASADI_SYMBOL_EXPORT casadi_int jac_Q_xyz_func_n_in(void) { + return 2; +} -extern "C" CASADI_SYMBOL_EXPORT casadi_int jac_Q_xyz_func_n_out(void) { return 1;} +extern "C" CASADI_SYMBOL_EXPORT casadi_int jac_Q_xyz_func_n_out(void) { + return 1; +} -extern "C" CASADI_SYMBOL_EXPORT casadi_real jac_Q_xyz_func_default_in(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT casadi_real +jac_Q_xyz_func_default_in(casadi_int i) { switch (i) { - default: return 0; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT const char* jac_Q_xyz_func_name_in(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT const char * +jac_Q_xyz_func_name_in(casadi_int i) { switch (i) { - case 0: return "i0"; - case 1: return "out_o0"; - default: return 0; + case 0: + return "i0"; + case 1: + return "out_o0"; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT const char* jac_Q_xyz_func_name_out(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT const char * +jac_Q_xyz_func_name_out(casadi_int i) { switch (i) { - case 0: return "jac_o0_i0"; - default: return 0; + case 0: + return "jac_o0_i0"; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT const casadi_int* jac_Q_xyz_func_sparsity_in(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT const casadi_int * +jac_Q_xyz_func_sparsity_in(casadi_int i) { switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s7; - default: return 0; + case 0: + return casadi_s0; + case 1: + return casadi_s7; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT const casadi_int* jac_Q_xyz_func_sparsity_out(casadi_int i) { +extern "C" CASADI_SYMBOL_EXPORT const casadi_int * +jac_Q_xyz_func_sparsity_out(casadi_int i) { switch (i) { - case 0: return casadi_s8; - default: return 0; + case 0: + return casadi_s8; + default: + return 0; } } -extern "C" CASADI_SYMBOL_EXPORT int jac_Q_xyz_func_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 5; - if (sz_res) *sz_res = 190; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 2169; +extern "C" CASADI_SYMBOL_EXPORT int jac_Q_xyz_func_work(casadi_int *sz_arg, + casadi_int *sz_res, + casadi_int *sz_iw, + casadi_int *sz_w) { + if (sz_arg) + *sz_arg = 5; + if (sz_res) + *sz_res = 190; + if (sz_iw) + *sz_iw = 0; + if (sz_w) + *sz_w = 2169; return 0; } - - diff --git a/controller/draco_controller/draco_task/draco_wbo_task_helper.h b/controller/draco_controller/draco_task/draco_wbo_task_helper.h index 38e2f4ae..fa1922b7 100644 --- a/controller/draco_controller/draco_task/draco_wbo_task_helper.h +++ b/controller/draco_controller/draco_task/draco_wbo_task_helper.h @@ -1,5 +1,5 @@ /* This file was automatically generated by CasADi. - * It consists of: + * 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 @@ -13,7 +13,8 @@ #define casadi_int long long int #endif -extern "C" int Q_xyz_func(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +extern "C" int Q_xyz_func(const casadi_real **arg, casadi_real **res, + casadi_int *iw, casadi_real *w, int mem); extern "C" int Q_xyz_func_alloc_mem(void); extern "C" int Q_xyz_func_init_mem(int mem); extern "C" void Q_xyz_func_free_mem(int mem); @@ -24,16 +25,18 @@ extern "C" void Q_xyz_func_decref(void); extern "C" casadi_int Q_xyz_func_n_in(void); extern "C" casadi_int Q_xyz_func_n_out(void); extern "C" casadi_real Q_xyz_func_default_in(casadi_int i); -extern "C" const char* Q_xyz_func_name_in(casadi_int i); -extern "C" const char* Q_xyz_func_name_out(casadi_int i); -extern "C" const casadi_int* Q_xyz_func_sparsity_in(casadi_int i); -extern "C" const casadi_int* Q_xyz_func_sparsity_out(casadi_int i); -extern "C" int Q_xyz_func_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +extern "C" const char *Q_xyz_func_name_in(casadi_int i); +extern "C" const char *Q_xyz_func_name_out(casadi_int i); +extern "C" const casadi_int *Q_xyz_func_sparsity_in(casadi_int i); +extern "C" const casadi_int *Q_xyz_func_sparsity_out(casadi_int i); +extern "C" int Q_xyz_func_work(casadi_int *sz_arg, casadi_int *sz_res, + casadi_int *sz_iw, casadi_int *sz_w); #define Q_xyz_func_SZ_ARG 190 #define Q_xyz_func_SZ_RES 2 #define Q_xyz_func_SZ_IW 0 #define Q_xyz_func_SZ_W 969 -extern "C" int jac_Q_xyz_func(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +extern "C" int jac_Q_xyz_func(const casadi_real **arg, casadi_real **res, + casadi_int *iw, casadi_real *w, int mem); extern "C" int jac_Q_xyz_func_alloc_mem(void); extern "C" int jac_Q_xyz_func_init_mem(int mem); extern "C" void jac_Q_xyz_func_free_mem(int mem); @@ -44,11 +47,12 @@ extern "C" void jac_Q_xyz_func_decref(void); extern "C" casadi_int jac_Q_xyz_func_n_in(void); extern "C" casadi_int jac_Q_xyz_func_n_out(void); extern "C" casadi_real jac_Q_xyz_func_default_in(casadi_int i); -extern "C" const char* jac_Q_xyz_func_name_in(casadi_int i); -extern "C" const char* jac_Q_xyz_func_name_out(casadi_int i); -extern "C" const casadi_int* jac_Q_xyz_func_sparsity_in(casadi_int i); -extern "C" const casadi_int* jac_Q_xyz_func_sparsity_out(casadi_int i); -extern "C" int jac_Q_xyz_func_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +extern "C" const char *jac_Q_xyz_func_name_in(casadi_int i); +extern "C" const char *jac_Q_xyz_func_name_out(casadi_int i); +extern "C" const casadi_int *jac_Q_xyz_func_sparsity_in(casadi_int i); +extern "C" const casadi_int *jac_Q_xyz_func_sparsity_out(casadi_int i); +extern "C" int jac_Q_xyz_func_work(casadi_int *sz_arg, casadi_int *sz_res, + casadi_int *sz_iw, casadi_int *sz_w); #define jac_Q_xyz_func_SZ_ARG 5 #define jac_Q_xyz_func_SZ_RES 190 #define jac_Q_xyz_func_SZ_IW 0 diff --git a/controller/draco_controller/state_estimator.hpp b/controller/draco_controller/state_estimator.hpp index 2b49c192..de1e0289 100644 --- a/controller/draco_controller/state_estimator.hpp +++ b/controller/draco_controller/state_estimator.hpp @@ -5,7 +5,7 @@ class PinocchioRobotSystem; class StateEstimator { public: - StateEstimator(PinocchioRobotSystem *robot) : robot_(robot){}; + StateEstimator(PinocchioRobotSystem *robot) : robot_(robot) {}; virtual ~StateEstimator() = default; virtual void Initialize(DracoSensorData *sensor_Data) = 0; diff --git a/controller/interface.hpp b/controller/interface.hpp index 4f0c7a9c..bedde83d 100644 --- a/controller/interface.hpp +++ b/controller/interface.hpp @@ -7,7 +7,7 @@ class InterruptHandler; class Interface { public: - Interface() : count_(0), running_time_(0.){}; + Interface() : count_(0), running_time_(0.) {}; virtual ~Interface() = default; diff --git a/mujoco/CMakeLists.txt b/mujoco/CMakeLists.txt index 8b137891..e69de29b 100644 --- a/mujoco/CMakeLists.txt +++ b/mujoco/CMakeLists.txt @@ -1 +0,0 @@ - diff --git a/mujoco/include/mujoco/mjdata.h b/mujoco/include/mujoco/mjdata.h index 65886b38..9b4a26c0 100644 --- a/mujoco/include/mujoco/mjdata.h +++ b/mujoco/include/mujoco/mjdata.h @@ -18,439 +18,501 @@ #include #include -#include #include #include +#include -//---------------------------------- primitive types (mjt) ----------------------------------------- - -typedef enum mjtState_ { // state elements - mjSTATE_TIME = 1<<0, // time - mjSTATE_QPOS = 1<<1, // position - mjSTATE_QVEL = 1<<2, // velocity - mjSTATE_ACT = 1<<3, // actuator activation - mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart - mjSTATE_CTRL = 1<<5, // control - mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force - mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque - mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints - mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies - mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies - mjSTATE_USERDATA = 1<<11, // user data - mjSTATE_PLUGIN = 1<<12, // plugin state - - mjNSTATE = 13, // number of state elements +//---------------------------------- primitive types (mjt) +//----------------------------------------- + +typedef enum mjtState_ { // state elements + mjSTATE_TIME = 1 << 0, // time + mjSTATE_QPOS = 1 << 1, // position + mjSTATE_QVEL = 1 << 2, // velocity + mjSTATE_ACT = 1 << 3, // actuator activation + mjSTATE_WARMSTART = 1 << 4, // acceleration used for warmstart + mjSTATE_CTRL = 1 << 5, // control + mjSTATE_QFRC_APPLIED = 1 << 6, // applied generalized force + mjSTATE_XFRC_APPLIED = 1 << 7, // applied Cartesian force/torque + mjSTATE_EQ_ACTIVE = 1 << 8, // enable/disable constraints + mjSTATE_MOCAP_POS = 1 << 9, // positions of mocap bodies + mjSTATE_MOCAP_QUAT = 1 << 10, // orientations of mocap bodies + mjSTATE_USERDATA = 1 << 11, // user data + mjSTATE_PLUGIN = 1 << 12, // plugin state + + mjNSTATE = 13, // number of state elements // convenience values for commonly used state specifications - mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT, - mjSTATE_FULLPHYSICS = mjSTATE_PHYSICS | mjSTATE_TIME | mjSTATE_PLUGIN, - mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED | - mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | - mjSTATE_USERDATA, - mjSTATE_INTEGRATION = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART + mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT, + mjSTATE_FULLPHYSICS = mjSTATE_PHYSICS | mjSTATE_TIME | mjSTATE_PLUGIN, + mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED | + mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | + mjSTATE_USERDATA, + mjSTATE_INTEGRATION = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART } mjtState; - -typedef enum mjtWarning_ { // warning types - mjWARN_INERTIA = 0, // (near) singular inertia matrix - mjWARN_CONTACTFULL, // too many contacts in contact list - mjWARN_CNSTRFULL, // too many constraints - mjWARN_VGEOMFULL, // too many visual geoms - mjWARN_BADQPOS, // bad number in qpos - mjWARN_BADQVEL, // bad number in qvel - mjWARN_BADQACC, // bad number in qacc - mjWARN_BADCTRL, // bad number in ctrl - - mjNWARNING // number of warnings +typedef enum mjtWarning_ { // warning types + mjWARN_INERTIA = 0, // (near) singular inertia matrix + mjWARN_CONTACTFULL, // too many contacts in contact list + mjWARN_CNSTRFULL, // too many constraints + mjWARN_VGEOMFULL, // too many visual geoms + mjWARN_BADQPOS, // bad number in qpos + mjWARN_BADQVEL, // bad number in qvel + mjWARN_BADQACC, // bad number in qacc + mjWARN_BADCTRL, // bad number in ctrl + + mjNWARNING // number of warnings } mjtWarning; - -typedef enum mjtTimer_ { // internal timers +typedef enum mjtTimer_ { // internal timers // main api - mjTIMER_STEP = 0, // step - mjTIMER_FORWARD, // forward - mjTIMER_INVERSE, // inverse + mjTIMER_STEP = 0, // step + mjTIMER_FORWARD, // forward + mjTIMER_INVERSE, // inverse // breakdown of step/forward - mjTIMER_POSITION, // fwdPosition - mjTIMER_VELOCITY, // fwdVelocity - mjTIMER_ACTUATION, // fwdActuation - mjTIMER_CONSTRAINT, // fwdConstraint - mjTIMER_ADVANCE, // mj_Euler, mj_implicit + mjTIMER_POSITION, // fwdPosition + mjTIMER_VELOCITY, // fwdVelocity + mjTIMER_ACTUATION, // fwdActuation + mjTIMER_CONSTRAINT, // fwdConstraint + mjTIMER_ADVANCE, // mj_Euler, mj_implicit // breakdown of fwdPosition - mjTIMER_POS_KINEMATICS, // kinematics, com, tendon, transmission - mjTIMER_POS_INERTIA, // inertia computations - mjTIMER_POS_COLLISION, // collision detection - mjTIMER_POS_MAKE, // make constraints - mjTIMER_POS_PROJECT, // project constraints + mjTIMER_POS_KINEMATICS, // kinematics, com, tendon, transmission + mjTIMER_POS_INERTIA, // inertia computations + mjTIMER_POS_COLLISION, // collision detection + mjTIMER_POS_MAKE, // make constraints + mjTIMER_POS_PROJECT, // project constraints // breakdown of mj_collision - mjTIMER_COL_BROAD, // broadphase - mjTIMER_COL_NARROW, // narrowphase + mjTIMER_COL_BROAD, // broadphase + mjTIMER_COL_NARROW, // narrowphase - mjNTIMER // number of timers + mjNTIMER // number of timers } mjtTimer; +//---------------------------------- mjContact +//----------------------------------------------------- -//---------------------------------- mjContact ----------------------------------------------------- - -struct mjContact_ { // result of collision detection functions +struct mjContact_ { // result of collision detection functions // contact parameters set by near-phase collision function - mjtNum dist; // distance between nearest points; neg: penetration - mjtNum pos[3]; // position of contact point: midpoint between geoms - mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1] + mjtNum dist; // distance between nearest points; neg: penetration + mjtNum pos[3]; // position of contact point: midpoint between geoms + mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1] // contact parameters set by mj_collideGeoms - mjtNum includemargin; // include if distplugin, required for deletion (nplugin x 1) - uintptr_t* plugin_data; // pointer to plugin-managed data structure (nplugin x 1) + int *plugin; // copy of m->plugin, required for deletion (nplugin x 1) + uintptr_t *plugin_data; // pointer to plugin-managed data structure (nplugin x + // 1) //-------------------- POSITION dependent // computed by mj_fwdPosition/mj_kinematics - mjtNum* xpos; // Cartesian position of body frame (nbody x 3) - mjtNum* xquat; // Cartesian orientation of body frame (nbody x 4) - mjtNum* xmat; // Cartesian orientation of body frame (nbody x 9) - mjtNum* xipos; // Cartesian position of body com (nbody x 3) - mjtNum* ximat; // Cartesian orientation of body inertia (nbody x 9) - mjtNum* xanchor; // Cartesian position of joint anchor (njnt x 3) - mjtNum* xaxis; // Cartesian joint axis (njnt x 3) - mjtNum* geom_xpos; // Cartesian geom position (ngeom x 3) - mjtNum* geom_xmat; // Cartesian geom orientation (ngeom x 9) - mjtNum* site_xpos; // Cartesian site position (nsite x 3) - mjtNum* site_xmat; // Cartesian site orientation (nsite x 9) - mjtNum* cam_xpos; // Cartesian camera position (ncam x 3) - mjtNum* cam_xmat; // Cartesian camera orientation (ncam x 9) - mjtNum* light_xpos; // Cartesian light position (nlight x 3) - mjtNum* light_xdir; // Cartesian light direction (nlight x 3) + mjtNum *xpos; // Cartesian position of body frame (nbody x 3) + mjtNum *xquat; // Cartesian orientation of body frame (nbody x 4) + mjtNum *xmat; // Cartesian orientation of body frame (nbody x 9) + mjtNum *xipos; // Cartesian position of body com (nbody x 3) + mjtNum *ximat; // Cartesian orientation of body inertia (nbody x 9) + mjtNum + *xanchor; // Cartesian position of joint anchor (njnt x 3) + mjtNum *xaxis; // Cartesian joint axis (njnt x 3) + mjtNum * + geom_xpos; // Cartesian geom position (ngeom x 3) + mjtNum * + geom_xmat; // Cartesian geom orientation (ngeom x 9) + mjtNum * + site_xpos; // Cartesian site position (nsite x 3) + mjtNum * + site_xmat; // Cartesian site orientation (nsite x 9) + mjtNum + *cam_xpos; // Cartesian camera position (ncam x 3) + mjtNum + *cam_xmat; // Cartesian camera orientation (ncam x 9) + mjtNum *light_xpos; // Cartesian light position (nlight x 3) + mjtNum *light_xdir; // Cartesian light direction (nlight x 3) // computed by mj_fwdPosition/mj_comPos - mjtNum* subtree_com; // center of mass of each subtree (nbody x 3) - mjtNum* cdof; // com-based motion axis of each dof (rot:lin) (nv x 6) - mjtNum* cinert; // com-based body inertia and mass (nbody x 10) + mjtNum *subtree_com; // center of mass of each subtree (nbody x 3) + mjtNum *cdof; // com-based motion axis of each dof (rot:lin) (nv x 6) + mjtNum + *cinert; // com-based body inertia and mass (nbody x 10) // computed by mj_fwdPosition/mj_flex - mjtNum* flexvert_xpos; // Cartesian flex vertex positions (nflexvert x 3) - mjtNum* flexelem_aabb; // flex element bounding boxes (center, size) (nflexelem x 6) - int* flexedge_J_rownnz; // number of non-zeros in Jacobian row (nflexedge x 1) - int* flexedge_J_rowadr; // row start address in colind array (nflexedge x 1) - int* flexedge_J_colind; // column indices in sparse Jacobian (nflexedge x nv) - mjtNum* flexedge_J; // flex edge Jacobian (nflexedge x nv) - mjtNum* flexedge_length; // flex edge lengths (nflexedge x 1) + mjtNum *flexvert_xpos; // Cartesian flex vertex positions (nflexvert x 3) + mjtNum *flexelem_aabb; // flex element bounding boxes (center, size) + // (nflexelem x 6) + int *flexedge_J_rownnz; // number of non-zeros in Jacobian row (nflexedge x 1) + int *flexedge_J_rowadr; // row start address in colind array (nflexedge x 1) + int *flexedge_J_colind; // column indices in sparse Jacobian (nflexedge x nv) + mjtNum *flexedge_J; // flex edge Jacobian (nflexedge x nv) + mjtNum *flexedge_length; // flex edge lengths (nflexedge x 1) // computed by mj_fwdPosition/mj_tendon - int* ten_wrapadr; // start address of tendon's path (ntendon x 1) - int* ten_wrapnum; // number of wrap points in path (ntendon x 1) - int* ten_J_rownnz; // number of non-zeros in Jacobian row (ntendon x 1) - int* ten_J_rowadr; // row start address in colind array (ntendon x 1) - int* ten_J_colind; // column indices in sparse Jacobian (ntendon x nv) - mjtNum* ten_J; // tendon Jacobian (ntendon x nv) - mjtNum* ten_length; // tendon lengths (ntendon x 1) - int* wrap_obj; // geom id; -1: site; -2: pulley (nwrap*2 x 1) - mjtNum* wrap_xpos; // Cartesian 3D points in all path (nwrap*2 x 3) + int *ten_wrapadr; // start address of tendon's path (ntendon + // x 1) + int *ten_wrapnum; // number of wrap points in path (ntendon + // x 1) + int *ten_J_rownnz; // number of non-zeros in Jacobian row (ntendon x 1) + int *ten_J_rowadr; // row start address in colind array (ntendon x 1) + int *ten_J_colind; // column indices in sparse Jacobian (ntendon x nv) + mjtNum + *ten_J; // tendon Jacobian (ntendon x nv) + mjtNum *ten_length; // tendon lengths (ntendon x 1) + int *wrap_obj; // geom id; -1: site; -2: pulley (nwrap*2 x + // 1) + mjtNum *wrap_xpos; // Cartesian 3D points in all path (nwrap*2 x 3) // computed by mj_fwdPosition/mj_transmission - mjtNum* actuator_length; // actuator lengths (nu x 1) - mjtNum* actuator_moment; // actuator moments (nu x nv) + mjtNum *actuator_length; // actuator lengths (nu x 1) + mjtNum *actuator_moment; // actuator moments (nu x nv) // computed by mj_fwdPosition/mj_crb - mjtNum* crb; // com-based composite inertia and mass (nbody x 10) - mjtNum* qM; // total inertia (sparse) (nM x 1) + mjtNum *crb; // com-based composite inertia and mass (nbody x 10) + mjtNum *qM; // total inertia (sparse) (nM x 1) // computed by mj_fwdPosition/mj_factorM - mjtNum* qLD; // L'*D*L factorization of M (sparse) (nM x 1) - mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) - mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1) + mjtNum *qLD; // L'*D*L factorization of M (sparse) (nM x 1) + mjtNum + *qLDiagInv; // 1/diag(D) (nv x 1) + mjtNum *qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv + // x 1) // computed by mj_collisionTree - mjtNum* bvh_aabb_dyn; // global bounding box (center, size) (nbvhdynamic x 6) - mjtByte* bvh_active; // volume has been added to collisions (nbvh x 1) + mjtNum *bvh_aabb_dyn; // global bounding box (center, size) (nbvhdynamic x 6) + mjtByte * + bvh_active; // volume has been added to collisions (nbvh x 1) //-------------------- POSITION, VELOCITY dependent // computed by mj_fwdVelocity - mjtNum* flexedge_velocity; // flex edge velocities (nflexedge x 1) - mjtNum* ten_velocity; // tendon velocities (ntendon x 1) - mjtNum* actuator_velocity; // actuator velocities (nu x 1) + mjtNum *flexedge_velocity; // flex edge velocities (nflexedge x 1) + mjtNum *ten_velocity; // tendon velocities (ntendon x 1) + mjtNum *actuator_velocity; // actuator velocities (nu x 1) // computed by mj_fwdVelocity/mj_comVel - mjtNum* cvel; // com-based velocity (rot:lin) (nbody x 6) - mjtNum* cdof_dot; // time-derivative of cdof (rot:lin) (nv x 6) + mjtNum *cvel; // com-based velocity (rot:lin) (nbody x 6) + mjtNum *cdof_dot; // time-derivative of cdof (rot:lin) (nv x 6) // computed by mj_fwdVelocity/mj_rne (without acceleration) - mjtNum* qfrc_bias; // C(qpos,qvel) (nv x 1) + mjtNum + *qfrc_bias; // C(qpos,qvel) (nv x 1) // computed by mj_fwdVelocity/mj_passive - mjtNum* qfrc_spring; // passive spring force (nv x 1) - mjtNum* qfrc_damper; // passive damper force (nv x 1) - mjtNum* qfrc_gravcomp; // passive gravity compensation force (nv x 1) - mjtNum* qfrc_fluid; // passive fluid force (nv x 1) - mjtNum* qfrc_passive; // total passive force (nv x 1) + mjtNum + *qfrc_spring; // passive spring force (nv x 1) + mjtNum + *qfrc_damper; // passive damper force (nv x 1) + mjtNum *qfrc_gravcomp; // passive gravity compensation force (nv + // x 1) + mjtNum + *qfrc_fluid; // passive fluid force (nv x 1) + mjtNum * + qfrc_passive; // total passive force (nv x 1) // computed by mj_sensorVel/mj_subtreeVel if needed - mjtNum* subtree_linvel; // linear velocity of subtree com (nbody x 3) - mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3) + mjtNum *subtree_linvel; // linear velocity of subtree com (nbody x 3) + mjtNum *subtree_angmom; // angular momentum about subtree com (nbody x 3) // computed by mj_Euler or mj_implicit - mjtNum* qH; // L'*D*L factorization of modified M (nM x 1) - mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1) + mjtNum *qH; // L'*D*L factorization of modified M (nM x 1) + mjtNum + *qHDiagInv; // 1/diag(D) of modified M (nv x 1) // computed by mj_resetData - int* D_rownnz; // non-zeros in each row (nv x 1) - int* D_rowadr; // address of each row in D_colind (nv x 1) - int* D_colind; // column indices of non-zeros (nD x 1) - int* B_rownnz; // non-zeros in each row (nbody x 1) - int* B_rowadr; // address of each row in B_colind (nbody x 1) - int* B_colind; // column indices of non-zeros (nB x 1) + int *D_rownnz; // non-zeros in each row (nv x 1) + int *D_rowadr; // address of each row in D_colind (nv x 1) + int *D_colind; // column indices of non-zeros (nD x 1) + int *B_rownnz; // non-zeros in each row (nbody x 1) + int *B_rowadr; // address of each row in B_colind (nbody x 1) + int *B_colind; // column indices of non-zeros (nB x 1) // computed by mj_implicit/mj_derivative - mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1) + mjtNum *qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1) // computed by mj_implicit/mju_factorLUSparse - mjtNum* qLU; // sparse LU of (qM - dt*qDeriv) (nD x 1) + mjtNum *qLU; // sparse LU of (qM - dt*qDeriv) (nD x 1) //-------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent // computed by mj_fwdActuation - mjtNum* actuator_force; // actuator force in actuation space (nu x 1) - mjtNum* qfrc_actuator; // actuator force (nv x 1) + mjtNum *actuator_force; // actuator force in actuation space (nu x 1) + mjtNum *qfrc_actuator; // actuator force (nv + // x 1) // computed by mj_fwdAcceleration - mjtNum* qfrc_smooth; // net unconstrained force (nv x 1) - mjtNum* qacc_smooth; // unconstrained acceleration (nv x 1) + mjtNum + *qfrc_smooth; // net unconstrained force (nv x 1) + mjtNum + *qacc_smooth; // unconstrained acceleration (nv x 1) // computed by mj_fwdConstraint/mj_inverse - mjtNum* qfrc_constraint; // constraint force (nv x 1) + mjtNum *qfrc_constraint; // constraint force (nv x 1) // computed by mj_inverse - mjtNum* qfrc_inverse; // net external force; should equal: (nv x 1) - // qfrc_applied + J'*xfrc_applied + qfrc_actuator + mjtNum *qfrc_inverse; // net external force; should equal: (nv + // x 1) qfrc_applied + J'*xfrc_applied + qfrc_actuator - // computed by mj_sensorAcc/mj_rnePostConstraint if needed; rotation:translation format - mjtNum* cacc; // com-based acceleration (nbody x 6) - mjtNum* cfrc_int; // com-based interaction force with parent (nbody x 6) - mjtNum* cfrc_ext; // com-based external force on body (nbody x 6) + // computed by mj_sensorAcc/mj_rnePostConstraint if needed; + // rotation:translation format + mjtNum *cacc; // com-based acceleration (nbody x 6) + mjtNum + *cfrc_int; // com-based interaction force with parent (nbody x 6) + mjtNum + *cfrc_ext; // com-based external force on body (nbody x 6) //-------------------- arena-allocated: POSITION dependent // computed by mj_collision - mjContact* contact; // list of all detected contacts (ncon x 1) + mjContact + *contact; // list of all detected contacts (ncon x 1) // computed by mj_makeConstraint - int* efc_type; // constraint type (mjtConstraint) (nefc x 1) - int* efc_id; // id of object of specified type (nefc x 1) - int* efc_J_rownnz; // number of non-zeros in constraint Jacobian row (nefc x 1) - int* efc_J_rowadr; // row start address in colind array (nefc x 1) - int* efc_J_rowsuper; // number of subsequent rows in supernode (nefc x 1) - int* efc_J_colind; // column indices in constraint Jacobian (nnzJ x 1) - int* efc_JT_rownnz; // number of non-zeros in constraint Jacobian row T (nv x 1) - int* efc_JT_rowadr; // row start address in colind array T (nv x 1) - int* efc_JT_rowsuper; // number of subsequent rows in supernode T (nv x 1) - int* efc_JT_colind; // column indices in constraint Jacobian T (nnzJ x 1) - mjtNum* efc_J; // constraint Jacobian (nnzJ x 1) - mjtNum* efc_JT; // constraint Jacobian transposed (nnzJ x 1) - mjtNum* efc_pos; // constraint position (equality, contact) (nefc x 1) - mjtNum* efc_margin; // inclusion margin (contact) (nefc x 1) - mjtNum* efc_frictionloss; // frictionloss (friction) (nefc x 1) - mjtNum* efc_diagApprox; // approximation to diagonal of A (nefc x 1) - mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (nefc x 4) - mjtNum* efc_D; // constraint mass (nefc x 1) - mjtNum* efc_R; // inverse constraint mass (nefc x 1) - int* tendon_efcadr; // first efc address involving tendon; -1: none (ntendon x 1) + int *efc_type; // constraint type (mjtConstraint) (nefc x 1) + int *efc_id; // id of object of specified type (nefc x 1) + int *efc_J_rownnz; // number of non-zeros in constraint Jacobian row (nefc x + // 1) + int *efc_J_rowadr; // row start address in colind array (nefc x + // 1) + int *efc_J_rowsuper; // number of subsequent rows in supernode (nefc + // x 1) + int *efc_J_colind; // column indices in constraint Jacobian (nnzJ x + // 1) + int *efc_JT_rownnz; // number of non-zeros in constraint Jacobian row T (nv x + // 1) + int *efc_JT_rowadr; // row start address in colind array T (nv x + // 1) + int *efc_JT_rowsuper; // number of subsequent rows in supernode T (nv + // x 1) + int *efc_JT_colind; // column indices in constraint Jacobian T (nnzJ + // x 1) + mjtNum *efc_J; // constraint Jacobian (nnzJ x 1) + mjtNum *efc_JT; // constraint Jacobian transposed (nnzJ x 1) + mjtNum + *efc_pos; // constraint position (equality, contact) (nefc x 1) + mjtNum * + efc_margin; // inclusion margin (contact) (nefc x 1) + mjtNum *efc_frictionloss; // frictionloss (friction) (nefc x 1) + mjtNum *efc_diagApprox; // approximation to diagonal of A (nefc x 1) + mjtNum + *efc_KBIP; // stiffness, damping, impedance, imp' (nefc x 4) + mjtNum *efc_D; // constraint mass (nefc x 1) + mjtNum *efc_R; // inverse constraint mass (nefc x 1) + int *tendon_efcadr; // first efc address involving tendon; -1: none (ntendon x + // 1) // computed by mj_island - int* dof_island; // island id of this dof; -1: none (nv x 1) - int* island_dofnum; // number of dofs in island (nisland x 1) - int* island_dofadr; // start address in island_dofind (nisland x 1) - int* island_dofind; // island dof indices; -1: none (nv x 1) - int* dof_islandind; // dof island indices; -1: none (nv x 1) - int* efc_island; // island id of this constraint (nefc x 1) - int* island_efcnum; // number of constraints in island (nisland x 1) - int* island_efcadr; // start address in island_efcind (nisland x 1) - int* island_efcind; // island constraint indices (nefc x 1) + int *dof_island; // island id of this dof; -1: none (nv x 1) + int *island_dofnum; // number of dofs in island (nisland x 1) + int *island_dofadr; // start address in island_dofind (nisland x 1) + int *island_dofind; // island dof indices; -1: none (nv x + // 1) + int *dof_islandind; // dof island indices; -1: none (nv x + // 1) + int * + efc_island; // island id of this constraint (nefc x 1) + int *island_efcnum; // number of constraints in island (nisland x 1) + int *island_efcadr; // start address in island_efcind (nisland x 1) + int *island_efcind; // island constraint indices (nefc + // x 1) // computed by mj_projectConstraint (dual solver) - int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1) - int* efc_AR_rowadr; // row start address in colind array (nefc x 1) - int* efc_AR_colind; // column indices in sparse AR (nefc x nefc) - mjtNum* efc_AR; // J*inv(M)*J' + R (nefc x nefc) + int *efc_AR_rownnz; // number of non-zeros in AR (nefc + // x 1) + int *efc_AR_rowadr; // row start address in colind array (nefc + // x 1) + int *efc_AR_colind; // column indices in sparse AR (nefc + // x nefc) + mjtNum + *efc_AR; // J*inv(M)*J' + R (nefc x nefc) //-------------------- arena-allocated: POSITION, VELOCITY dependent // computed by mj_fwdVelocity/mj_referenceConstraint - mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1) - mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1) + mjtNum + *efc_vel; // velocity in constraint space: J*qvel (nefc x 1) + mjtNum + *efc_aref; // reference pseudo-acceleration (nefc x 1) - //-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent + //-------------------- arena-allocated: POSITION, VELOCITY, + //CONTROL/ACCELERATION dependent // computed by mj_fwdConstraint/mj_inverse - mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) - mjtNum* efc_force; // constraint force in constraint space (nefc x 1) - int* efc_state; // constraint state (mjtConstraintState) (nefc x 1) + mjtNum *efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) + mjtNum * + efc_force; // constraint force in constraint space (nefc x 1) + int * + efc_state; // constraint state (mjtConstraintState) (nefc x 1) // ThreadPool for multithreaded operations uintptr_t threadpool; }; typedef struct mjData_ mjData; - -//---------------------------------- callback function types --------------------------------------- +//---------------------------------- callback function types +//--------------------------------------- // generic MuJoCo function -typedef void (*mjfGeneric)(const mjModel* m, mjData* d); +typedef void (*mjfGeneric)(const mjModel *m, mjData *d); // contact filter: 1- discard, 0- collide -typedef int (*mjfConFilt)(const mjModel* m, mjData* d, int geom1, int geom2); +typedef int (*mjfConFilt)(const mjModel *m, mjData *d, int geom1, int geom2); // sensor simulation -typedef void (*mjfSensor)(const mjModel* m, mjData* d, int stage); +typedef void (*mjfSensor)(const mjModel *m, mjData *d, int stage); // timer typedef mjtNum (*mjfTime)(void); // actuator dynamics, gain, bias -typedef mjtNum (*mjfAct)(const mjModel* m, const mjData* d, int id); +typedef mjtNum (*mjfAct)(const mjModel *m, const mjData *d, int id); // collision detection -typedef int (*mjfCollision)(const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +typedef int (*mjfCollision)(const mjModel *m, const mjData *d, mjContact *con, + int g1, int g2, mjtNum margin); -#endif // MUJOCO_MJDATA_H_ +#endif // MUJOCO_MJDATA_H_ diff --git a/mujoco/include/mujoco/mjexport.h b/mujoco/include/mujoco/mjexport.h index 1194e03c..d208d036 100644 --- a/mujoco/include/mujoco/mjexport.h +++ b/mujoco/include/mujoco/mjexport.h @@ -16,32 +16,32 @@ #define MUJOCO_MJEXPORT_H_ #if defined _WIN32 || defined __CYGWIN__ - #define MUJOCO_HELPER_DLL_IMPORT __declspec(dllimport) - #define MUJOCO_HELPER_DLL_EXPORT __declspec(dllexport) - #define MUJOCO_HELPER_DLL_LOCAL +#define MUJOCO_HELPER_DLL_IMPORT __declspec(dllimport) +#define MUJOCO_HELPER_DLL_EXPORT __declspec(dllexport) +#define MUJOCO_HELPER_DLL_LOCAL #else - #if __GNUC__ >= 4 - #define MUJOCO_HELPER_DLL_IMPORT __attribute__ ((visibility ("default"))) - #define MUJOCO_HELPER_DLL_EXPORT __attribute__ ((visibility ("default"))) - #define MUJOCO_HELPER_DLL_LOCAL __attribute__ ((visibility ("hidden"))) - #else - #define MUJOCO_HELPER_DLL_IMPORT - #define MUJOCO_HELPER_DLL_EXPORT - #define MUJOCO_HELPER_DLL_LOCAL - #endif +#if __GNUC__ >= 4 +#define MUJOCO_HELPER_DLL_IMPORT __attribute__((visibility("default"))) +#define MUJOCO_HELPER_DLL_EXPORT __attribute__((visibility("default"))) +#define MUJOCO_HELPER_DLL_LOCAL __attribute__((visibility("hidden"))) +#else +#define MUJOCO_HELPER_DLL_IMPORT +#define MUJOCO_HELPER_DLL_EXPORT +#define MUJOCO_HELPER_DLL_LOCAL +#endif #endif #ifdef MJ_STATIC - // static library - #define MJAPI - #define MJLOCAL +// static library +#define MJAPI +#define MJLOCAL #else - #ifdef MUJOCO_DLL_EXPORTS - #define MJAPI MUJOCO_HELPER_DLL_EXPORT - #else - #define MJAPI MUJOCO_HELPER_DLL_IMPORT - #endif - #define MJLOCAL MUJOCO_HELPER_DLL_LOCAL +#ifdef MUJOCO_DLL_EXPORTS +#define MJAPI MUJOCO_HELPER_DLL_EXPORT +#else +#define MJAPI MUJOCO_HELPER_DLL_IMPORT +#endif +#define MJLOCAL MUJOCO_HELPER_DLL_LOCAL #endif -#endif // MUJOCO_MJEXPORT_H_ +#endif // MUJOCO_MJEXPORT_H_ diff --git a/mujoco/include/mujoco/mjmacro.h b/mujoco/include/mujoco/mjmacro.h index d6988292..138d7753 100644 --- a/mujoco/include/mujoco/mjmacro.h +++ b/mujoco/include/mujoco/mjmacro.h @@ -15,15 +15,16 @@ #ifndef MUJOCO_MJMACRO_H_ #define MUJOCO_MJMACRO_H_ -// include asan interface header, or provide stubs for poison/unpoison macros when not using asan +// include asan interface header, or provide stubs for poison/unpoison macros +// when not using asan #ifdef ADDRESS_SANITIZER - #include +#include #elif defined(_MSC_VER) - #define ASAN_POISON_MEMORY_REGION(addr, size) - #define ASAN_UNPOISON_MEMORY_REGION(addr, size) +#define ASAN_POISON_MEMORY_REGION(addr, size) +#define ASAN_UNPOISON_MEMORY_REGION(addr, size) #else - #define ASAN_POISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) - #define ASAN_UNPOISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) +#define ASAN_POISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) +#define ASAN_UNPOISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) #endif // max and min (use only for primitive types) @@ -32,18 +33,19 @@ // return current value of mjOption enable/disable flags #define mjDISABLED(x) (m->opt.disableflags & (x)) -#define mjENABLED(x) (m->opt.enableflags & (x)) +#define mjENABLED(x) (m->opt.enableflags & (x)) // is actuator disabled -#define mjACTUATORDISABLED(i) (m->opt.disableactuator & (1 << m->actuator_group[i])) +#define mjACTUATORDISABLED(i) \ + (m->opt.disableactuator & (1 << m->actuator_group[i])) // annotation for functions that accept printf-like variadic arguments #ifndef mjPRINTFLIKE - #if defined(__GNUC__) - #define mjPRINTFLIKE(n, m) __attribute__((format(printf, n, m))) - #else - #define mjPRINTFLIKE(n, m) - #endif +#if defined(__GNUC__) +#define mjPRINTFLIKE(n, m) __attribute__((format(printf, n, m))) +#else +#define mjPRINTFLIKE(n, m) +#endif #endif -#endif // MUJOCO_MJMACRO_H_ +#endif // MUJOCO_MJMACRO_H_ diff --git a/mujoco/include/mujoco/mjplugin.h b/mujoco/include/mujoco/mjplugin.h index b6897080..fd29bddf 100644 --- a/mujoco/include/mujoco/mjplugin.h +++ b/mujoco/include/mujoco/mjplugin.h @@ -20,159 +20,174 @@ #include #include - -//---------------------------------- Resource Provider --------------------------------------------- +//---------------------------------- Resource Provider +//--------------------------------------------- struct mjResource_ { - char* name; // name of resource (filename, etc) - void* data; // opaque data pointer - char timestamp[512]; // timestamp of the resource - const struct mjpResourceProvider* provider; // pointer to the provider + char *name; // name of resource (filename, etc) + void *data; // opaque data pointer + char timestamp[512]; // timestamp of the resource + const struct mjpResourceProvider *provider; // pointer to the provider }; typedef struct mjResource_ mjResource; // callback for opening a resource, returns zero on failure -typedef int (*mjfOpenResource)(mjResource* resource); +typedef int (*mjfOpenResource)(mjResource *resource); // callback for reading a resource // return number of bytes stored in buffer, return -1 if error -typedef int (*mjfReadResource)(mjResource* resource, const void** buffer); +typedef int (*mjfReadResource)(mjResource *resource, const void **buffer); -// callback for closing a resource (responsible for freeing any allocated memory) -typedef void (*mjfCloseResource)(mjResource* resource); +// callback for closing a resource (responsible for freeing any allocated +// memory) +typedef void (*mjfCloseResource)(mjResource *resource); // callback for returning the directory of a resource // sets dir to directory string with ndir being size of directory string -typedef void (*mjfGetResourceDir)(mjResource* resource, const char** dir, int* ndir); +typedef void (*mjfGetResourceDir)(mjResource *resource, const char **dir, + int *ndir); // callback for checking if the current resource was modified from the time // specified by the timestamp // returns 0 if the resource's timestamp matches the provided timestamp // returns > 0 if the the resource is younger than the given timestamp // returns < 0 if the resource is older than the given timestamp -typedef int (*mjfResourceModified)(const mjResource* resource, const char* timestamp); +typedef int (*mjfResourceModified)(const mjResource *resource, + const char *timestamp); // struct describing a single resource provider struct mjpResourceProvider { - const char* prefix; // prefix for match against a resource name - mjfOpenResource open; // opening callback - mjfReadResource read; // reading callback - mjfCloseResource close; // closing callback - mjfGetResourceDir getdir; // get directory callback (optional) - mjfResourceModified modified; // resource modified callback (optional) - void* data; // opaque data pointer (resource invariant) + const char *prefix; // prefix for match against a resource name + mjfOpenResource open; // opening callback + mjfReadResource read; // reading callback + mjfCloseResource close; // closing callback + mjfGetResourceDir getdir; // get directory callback (optional) + mjfResourceModified modified; // resource modified callback (optional) + void *data; // opaque data pointer (resource invariant) }; typedef struct mjpResourceProvider mjpResourceProvider; - -//---------------------------------- Plugins ------------------------------------------------------- +//---------------------------------- Plugins +//------------------------------------------------------- typedef enum mjtPluginCapabilityBit_ { - mjPLUGIN_ACTUATOR = 1<<0, // actuator forces - mjPLUGIN_SENSOR = 1<<1, // sensor measurements - mjPLUGIN_PASSIVE = 1<<2, // passive forces - mjPLUGIN_SDF = 1<<3, // signed distance fields + mjPLUGIN_ACTUATOR = 1 << 0, // actuator forces + mjPLUGIN_SENSOR = 1 << 1, // sensor measurements + mjPLUGIN_PASSIVE = 1 << 2, // passive forces + mjPLUGIN_SDF = 1 << 3, // signed distance fields } mjtPluginCapabilityBit; struct mjpPlugin_ { - const char* name; // globally unique name identifying the plugin + const char *name; // globally unique name identifying the plugin - int nattribute; // number of configuration attributes - const char* const* attributes; // name of configuration attributes + int nattribute; // number of configuration attributes + const char *const *attributes; // name of configuration attributes - int capabilityflags; // plugin capabilities: bitfield of mjtPluginCapabilityBit - int needstage; // sensor computation stage (mjtStage) + int capabilityflags; // plugin capabilities: bitfield of + // mjtPluginCapabilityBit + int needstage; // sensor computation stage (mjtStage) // number of mjtNums needed to store the state of a plugin instance (required) - int (*nstate)(const mjModel* m, int instance); + int (*nstate)(const mjModel *m, int instance); - // dimension of the specified sensor's output (required only for sensor plugins) - int (*nsensordata)(const mjModel* m, int instance, int sensor_id); + // dimension of the specified sensor's output (required only for sensor + // plugins) + int (*nsensordata)(const mjModel *m, int instance, int sensor_id); - // called when a new mjData is being created (required), returns 0 on success or -1 on failure - int (*init)(const mjModel* m, mjData* d, int instance); + // called when a new mjData is being created (required), returns 0 on success + // or -1 on failure + int (*init)(const mjModel *m, mjData *d, int instance); // called when an mjData is being freed (optional) - void (*destroy)(mjData* d, int instance); + void (*destroy)(mjData *d, int instance); // called when an mjData is being copied (optional) - void (*copy)(mjData* dest, const mjModel* m, const mjData* src, int instance); + void (*copy)(mjData *dest, const mjModel *m, const mjData *src, int instance); // called when an mjData is being reset (required) - void (*reset)(const mjModel* m, double* plugin_state, void* plugin_data, int instance); + void (*reset)(const mjModel *m, double *plugin_state, void *plugin_data, + int instance); // called when the plugin needs to update its outputs (required) - void (*compute)(const mjModel* m, mjData* d, int instance, int capability_bit); + void (*compute)(const mjModel *m, mjData *d, int instance, + int capability_bit); // called when time integration occurs (optional) - void (*advance)(const mjModel* m, mjData* d, int instance); + void (*advance)(const mjModel *m, mjData *d, int instance); // called by mjv_updateScene (optional) - void (*visualize)(const mjModel*m, mjData* d, const mjvOption* opt, mjvScene* scn, int instance); + void (*visualize)(const mjModel *m, mjData *d, const mjvOption *opt, + mjvScene *scn, int instance); // methods specific to actuators (optional) - // dimension of the actuator state for the plugin (excluding state from actuator's dyntype) - int (*actuator_actdim)(const mjModel*m, int instance, int actuator_id); + // dimension of the actuator state for the plugin (excluding state from + // actuator's dyntype) + int (*actuator_actdim)(const mjModel *m, int instance, int actuator_id); // updates the actuator plugin's entries in act_dot // called after native act_dot is computed and before the compute callback - void (*actuator_act_dot)(const mjModel* m, mjData* d, int instance); + void (*actuator_act_dot)(const mjModel *m, mjData *d, int instance); // methods specific to signed distance fields (optional) // signed distance from the surface - mjtNum (*sdf_distance)(const mjtNum point[3], const mjData* d, int instance); + mjtNum (*sdf_distance)(const mjtNum point[3], const mjData *d, int instance); // gradient of distance with respect to local coordinates - void (*sdf_gradient)(mjtNum gradient[3], const mjtNum point[3], const mjData* d, int instance); + void (*sdf_gradient)(mjtNum gradient[3], const mjtNum point[3], + const mjData *d, int instance); // called during compilation for marching cubes - mjtNum (*sdf_staticdistance)(const mjtNum point[3], const mjtNum* attributes); + mjtNum (*sdf_staticdistance)(const mjtNum point[3], const mjtNum *attributes); // convert attributes and provide defaults if not present - void (*sdf_attribute)(mjtNum attribute[], const char* name[], const char* value[]); + void (*sdf_attribute)(mjtNum attribute[], const char *name[], + const char *value[]); // bounding box of implicit surface - void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes); + void (*sdf_aabb)(mjtNum aabb[6], const mjtNum *attributes); }; typedef struct mjpPlugin_ mjpPlugin; #if defined(__has_attribute) - #if __has_attribute(constructor) - #define mjPLUGIN_LIB_INIT __attribute__((constructor)) static void _mjplugin_init(void) - #endif // __has_attribute(constructor) +#if __has_attribute(constructor) +#define mjPLUGIN_LIB_INIT \ + __attribute__((constructor)) static void _mjplugin_init(void) +#endif // __has_attribute(constructor) #elif defined(_MSC_VER) - #ifndef mjDLLMAIN - #define mjDLLMAIN DllMain - #endif - - #if !defined(mjEXTERNC) - #if defined(__cplusplus) - #define mjEXTERNC extern "C" - #else - #define mjEXTERNC - #endif // defined(__cplusplus) - #endif // !defined(mjEXTERNC) - - // NOLINTBEGIN(runtime/int) - #define mjPLUGIN_LIB_INIT \ - static void _mjplugin_dllmain(void); \ - mjEXTERNC int __stdcall mjDLLMAIN(void* hinst, unsigned long reason, void* reserved) { \ - if (reason == 1) { \ - _mjplugin_dllmain(); \ - } \ - return 1; \ - } \ - static void _mjplugin_dllmain(void) - // NOLINTEND(runtime/int) - -#endif // defined(_MSC_VER) +#ifndef mjDLLMAIN +#define mjDLLMAIN DllMain +#endif + +#if !defined(mjEXTERNC) +#if defined(__cplusplus) +#define mjEXTERNC extern "C" +#else +#define mjEXTERNC +#endif // defined(__cplusplus) +#endif // !defined(mjEXTERNC) + +// NOLINTBEGIN(runtime/int) +#define mjPLUGIN_LIB_INIT \ + static void _mjplugin_dllmain(void); \ + mjEXTERNC int __stdcall mjDLLMAIN(void *hinst, unsigned long reason, \ + void *reserved) { \ + if (reason == 1) { \ + _mjplugin_dllmain(); \ + } \ + return 1; \ + } \ + static void _mjplugin_dllmain(void) +// NOLINTEND(runtime/int) + +#endif // defined(_MSC_VER) // function pointer type for mj_loadAllPluginLibraries callback -typedef void (*mjfPluginLibraryLoadCallback)(const char* filename, int first, int count); +typedef void (*mjfPluginLibraryLoadCallback)(const char *filename, int first, + int count); -#endif // MUJOCO_INCLUDE_MJPLUGIN_H_ +#endif // MUJOCO_INCLUDE_MJPLUGIN_H_ diff --git a/mujoco/include/mujoco/mjrender.h b/mujoco/include/mujoco/mjrender.h index 4a4089d7..33400507 100644 --- a/mujoco/include/mujoco/mjrender.h +++ b/mujoco/include/mujoco/mjrender.h @@ -15,157 +15,155 @@ #ifndef MUJOCO_MJRENDER_H_ #define MUJOCO_MJRENDER_H_ - #if defined(__cplusplus) extern "C" { #endif -#define mjNAUX 10 // number of auxiliary buffers -#define mjMAXTEXTURE 1000 // maximum number of textures - - -//---------------------------------- primitive types (mjt) ----------------------------------------- - -typedef enum mjtGridPos_ { // grid position for overlay - mjGRID_TOPLEFT = 0, // top left - mjGRID_TOPRIGHT, // top right - mjGRID_BOTTOMLEFT, // bottom left - mjGRID_BOTTOMRIGHT, // bottom right - mjGRID_TOP, // top center - mjGRID_BOTTOM, // bottom center - mjGRID_LEFT, // left center - mjGRID_RIGHT // right center +#define mjNAUX 10 // number of auxiliary buffers +#define mjMAXTEXTURE 1000 // maximum number of textures + +//---------------------------------- primitive types (mjt) +//----------------------------------------- + +typedef enum mjtGridPos_ { // grid position for overlay + mjGRID_TOPLEFT = 0, // top left + mjGRID_TOPRIGHT, // top right + mjGRID_BOTTOMLEFT, // bottom left + mjGRID_BOTTOMRIGHT, // bottom right + mjGRID_TOP, // top center + mjGRID_BOTTOM, // bottom center + mjGRID_LEFT, // left center + mjGRID_RIGHT // right center } mjtGridPos; - -typedef enum mjtFramebuffer_ { // OpenGL framebuffer option - mjFB_WINDOW = 0, // default/window buffer - mjFB_OFFSCREEN // offscreen buffer +typedef enum mjtFramebuffer_ { // OpenGL framebuffer option + mjFB_WINDOW = 0, // default/window buffer + mjFB_OFFSCREEN // offscreen buffer } mjtFramebuffer; -typedef enum mjtDepthMap_ { // depth mapping for `mjr_readPixels` - mjDEPTH_ZERONEAR = 0, // standard depth map; 0: znear, 1: zfar - mjDEPTH_ZEROFAR = 1 // reversed depth map; 1: znear, 0: zfar +typedef enum mjtDepthMap_ { // depth mapping for `mjr_readPixels` + mjDEPTH_ZERONEAR = 0, // standard depth map; 0: znear, 1: zfar + mjDEPTH_ZEROFAR = 1 // reversed depth map; 1: znear, 0: zfar } mjtDepthMap; -typedef enum mjtFontScale_ { // font scale, used at context creation - mjFONTSCALE_50 = 50, // 50% scale, suitable for low-res rendering - mjFONTSCALE_100 = 100, // normal scale, suitable in the absence of DPI scaling - mjFONTSCALE_150 = 150, // 150% scale - mjFONTSCALE_200 = 200, // 200% scale - mjFONTSCALE_250 = 250, // 250% scale - mjFONTSCALE_300 = 300 // 300% scale +typedef enum mjtFontScale_ { // font scale, used at context creation + mjFONTSCALE_50 = 50, // 50% scale, suitable for low-res rendering + mjFONTSCALE_100 = 100, // normal scale, suitable in the absence of DPI scaling + mjFONTSCALE_150 = 150, // 150% scale + mjFONTSCALE_200 = 200, // 200% scale + mjFONTSCALE_250 = 250, // 250% scale + mjFONTSCALE_300 = 300 // 300% scale } mjtFontScale; - -typedef enum mjtFont_ { // font type, used at each text operation - mjFONT_NORMAL = 0, // normal font - mjFONT_SHADOW, // normal font with shadow (for higher contrast) - mjFONT_BIG // big font (for user alerts) +typedef enum mjtFont_ { // font type, used at each text operation + mjFONT_NORMAL = 0, // normal font + mjFONT_SHADOW, // normal font with shadow (for higher contrast) + mjFONT_BIG // big font (for user alerts) } mjtFont; - -struct mjrRect_ { // OpenGL rectangle - int left; // left (usually 0) - int bottom; // bottom (usually 0) - int width; // width (usually buffer width) - int height; // height (usually buffer height) +struct mjrRect_ { // OpenGL rectangle + int left; // left (usually 0) + int bottom; // bottom (usually 0) + int width; // width (usually buffer width) + int height; // height (usually buffer height) }; typedef struct mjrRect_ mjrRect; +//---------------------------------- mjrContext +//---------------------------------------------------- -//---------------------------------- mjrContext ---------------------------------------------------- - -struct mjrContext_ { // custom OpenGL context +struct mjrContext_ { // custom OpenGL context // parameters copied from mjVisual - float lineWidth; // line width for wireframe rendering - float shadowClip; // clipping radius for directional lights - float shadowScale; // fraction of light cutoff for spot lights - float fogStart; // fog start = stat.extent * vis.map.fogstart - float fogEnd; // fog end = stat.extent * vis.map.fogend - float fogRGBA[4]; // fog rgba - int shadowSize; // size of shadow map texture - int offWidth; // width of offscreen buffer - int offHeight; // height of offscreen buffer - int offSamples; // number of offscreen buffer multisamples + float lineWidth; // line width for wireframe rendering + float shadowClip; // clipping radius for directional lights + float shadowScale; // fraction of light cutoff for spot lights + float fogStart; // fog start = stat.extent * vis.map.fogstart + float fogEnd; // fog end = stat.extent * vis.map.fogend + float fogRGBA[4]; // fog rgba + int shadowSize; // size of shadow map texture + int offWidth; // width of offscreen buffer + int offHeight; // height of offscreen buffer + int offSamples; // number of offscreen buffer multisamples // parameters specified at creation - int fontScale; // font scale - int auxWidth[mjNAUX]; // auxiliary buffer width - int auxHeight[mjNAUX]; // auxiliary buffer height - int auxSamples[mjNAUX]; // auxiliary buffer multisamples + int fontScale; // font scale + int auxWidth[mjNAUX]; // auxiliary buffer width + int auxHeight[mjNAUX]; // auxiliary buffer height + int auxSamples[mjNAUX]; // auxiliary buffer multisamples // offscreen rendering objects - unsigned int offFBO; // offscreen framebuffer object - unsigned int offFBO_r; // offscreen framebuffer for resolving multisamples - unsigned int offColor; // offscreen color buffer - unsigned int offColor_r; // offscreen color buffer for resolving multisamples + unsigned int offFBO; // offscreen framebuffer object + unsigned int offFBO_r; // offscreen framebuffer for resolving multisamples + unsigned int offColor; // offscreen color buffer + unsigned int offColor_r; // offscreen color buffer for resolving multisamples unsigned int offDepthStencil; // offscreen depth and stencil buffer - unsigned int offDepthStencil_r; // offscreen depth and stencil buffer for resolving multisamples + unsigned int offDepthStencil_r; // offscreen depth and stencil buffer for + // resolving multisamples // shadow rendering objects - unsigned int shadowFBO; // shadow map framebuffer object - unsigned int shadowTex; // shadow map texture + unsigned int shadowFBO; // shadow map framebuffer object + unsigned int shadowTex; // shadow map texture // auxiliary buffers - unsigned int auxFBO[mjNAUX]; // auxiliary framebuffer object - unsigned int auxFBO_r[mjNAUX]; // auxiliary framebuffer object for resolving - unsigned int auxColor[mjNAUX]; // auxiliary color buffer - unsigned int auxColor_r[mjNAUX];// auxiliary color buffer for resolving + unsigned int auxFBO[mjNAUX]; // auxiliary framebuffer object + unsigned int auxFBO_r[mjNAUX]; // auxiliary framebuffer object for resolving + unsigned int auxColor[mjNAUX]; // auxiliary color buffer + unsigned int auxColor_r[mjNAUX]; // auxiliary color buffer for resolving // texture objects and info - int ntexture; // number of allocated textures - int textureType[100]; // type of texture (mjtTexture) (ntexture) - unsigned int texture[100]; // texture names + int ntexture; // number of allocated textures + int textureType[100]; // type of texture (mjtTexture) (ntexture) + unsigned int texture[100]; // texture names // displaylist starting positions - unsigned int basePlane; // all planes from model - unsigned int baseMesh; // all meshes from model - unsigned int baseHField; // all hfields from model - unsigned int baseBuiltin; // all buildin geoms, with quality from model - unsigned int baseFontNormal; // normal font - unsigned int baseFontShadow; // shadow font - unsigned int baseFontBig; // big font + unsigned int basePlane; // all planes from model + unsigned int baseMesh; // all meshes from model + unsigned int baseHField; // all hfields from model + unsigned int baseBuiltin; // all buildin geoms, with quality from model + unsigned int baseFontNormal; // normal font + unsigned int baseFontShadow; // shadow font + unsigned int baseFontBig; // big font // displaylist ranges - int rangePlane; // all planes from model - int rangeMesh; // all meshes from model - int rangeHField; // all hfields from model - int rangeBuiltin; // all builtin geoms, with quality from model - int rangeFont; // all characters in font + int rangePlane; // all planes from model + int rangeMesh; // all meshes from model + int rangeHField; // all hfields from model + int rangeBuiltin; // all builtin geoms, with quality from model + int rangeFont; // all characters in font // skin VBOs - int nskin; // number of skins - unsigned int* skinvertVBO; // skin vertex position VBOs (nskin) - unsigned int* skinnormalVBO; // skin vertex normal VBOs (nskin) - unsigned int* skintexcoordVBO; // skin vertex texture coordinate VBOs (nskin) - unsigned int* skinfaceVBO; // skin face index VBOs (nskin) + int nskin; // number of skins + unsigned int *skinvertVBO; // skin vertex position VBOs (nskin) + unsigned int *skinnormalVBO; // skin vertex normal VBOs (nskin) + unsigned int *skintexcoordVBO; // skin vertex texture coordinate VBOs (nskin) + unsigned int *skinfaceVBO; // skin face index VBOs (nskin) // character info - int charWidth[127]; // character widths: normal and shadow - int charWidthBig[127]; // chacarter widths: big - int charHeight; // character heights: normal and shadow - int charHeightBig; // character heights: big + int charWidth[127]; // character widths: normal and shadow + int charWidthBig[127]; // chacarter widths: big + int charHeight; // character heights: normal and shadow + int charHeightBig; // character heights: big // capabilities - int glInitialized; // is OpenGL initialized - int windowAvailable; // is default/window framebuffer available - int windowSamples; // number of samples for default/window framebuffer - int windowStereo; // is stereo available for default/window framebuffer - int windowDoublebuffer; // is default/window framebuffer double buffered + int glInitialized; // is OpenGL initialized + int windowAvailable; // is default/window framebuffer available + int windowSamples; // number of samples for default/window framebuffer + int windowStereo; // is stereo available for default/window framebuffer + int windowDoublebuffer; // is default/window framebuffer double buffered // framebuffer - int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN + int currentBuffer; // currently active framebuffer: mjFB_WINDOW or + // mjFB_OFFSCREEN // pixel output format - int readPixelFormat; // default color pixel format for mjr_readPixels + int readPixelFormat; // default color pixel format for mjr_readPixels // depth output format - int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR + int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR }; typedef struct mjrContext_ mjrContext; #if defined(__cplusplus) } #endif -#endif // MUJOCO_MJRENDER_H_ +#endif // MUJOCO_MJRENDER_H_ diff --git a/mujoco/include/mujoco/mjthread.h b/mujoco/include/mujoco/mjthread.h index 153ff0cc..46eb4019 100644 --- a/mujoco/include/mujoco/mjthread.h +++ b/mujoco/include/mujoco/mjthread.h @@ -15,28 +15,28 @@ #ifndef MUJOCO_INCLUDE_MJTHREAD_H_ #define MUJOCO_INCLUDE_MJTHREAD_H_ -#define mjMAXTHREAD 128 // maximum number of threads in a thread pool +#define mjMAXTHREAD 128 // maximum number of threads in a thread pool -typedef enum mjtTaskStatus_ { // status values for mjTask - mjTASK_NEW = 0, // newly created - mjTASK_QUEUED, // enqueued in a thread pool - mjTASK_COMPLETED // completed execution +typedef enum mjtTaskStatus_ { // status values for mjTask + mjTASK_NEW = 0, // newly created + mjTASK_QUEUED, // enqueued in a thread pool + mjTASK_COMPLETED // completed execution } mjtTaskStatus; // function pointer type for mjTask -typedef void* (*mjfTask)(void*); +typedef void *(*mjfTask)(void *); // An opaque type representing a thread pool. struct mjThreadPool_ { - int nworker; // number of workers in the pool + int nworker; // number of workers in the pool }; typedef struct mjThreadPool_ mjThreadPool; -struct mjTask_ { // a task that can be executed by a thread pool. - mjfTask func; // pointer to the function that implements the task - void* args; // arguments to func - volatile int status; // status of the task +struct mjTask_ { // a task that can be executed by a thread pool. + mjfTask func; // pointer to the function that implements the task + void *args; // arguments to func + volatile int status; // status of the task }; typedef struct mjTask_ mjTask; -#endif // MUJOCO_INCLUDE_MJTHREAD_H_ +#endif // MUJOCO_INCLUDE_MJTHREAD_H_ diff --git a/mujoco/include/mujoco/mjtnum.h b/mujoco/include/mujoco/mjtnum.h index b9d78ea1..89cc1b44 100644 --- a/mujoco/include/mujoco/mjtnum.h +++ b/mujoco/include/mujoco/mjtnum.h @@ -15,27 +15,24 @@ #ifndef MUJOCO_INCLUDE_MJTNUM_H_ #define MUJOCO_INCLUDE_MJTNUM_H_ -//---------------------------------- floating-point definition ------------------------------------- +//---------------------------------- floating-point definition +//------------------------------------- // compile-time configuration options -#define mjUSEDOUBLE // single or double precision for mjtNum - +#define mjUSEDOUBLE // single or double precision for mjtNum // floating point data type and minval #ifdef mjUSEDOUBLE - typedef double mjtNum; - #define mjMINVAL 1E-15 // minimum value in any denominator +typedef double mjtNum; +#define mjMINVAL 1E-15 // minimum value in any denominator #else - typedef float mjtNum; - #define mjMINVAL 1E-15f +typedef float mjtNum; +#define mjMINVAL 1E-15f #endif +//-------------------------------------- byte definition +//------------------------------------------- +typedef unsigned char mjtByte; // used for true/false -//-------------------------------------- byte definition ------------------------------------------- - -typedef unsigned char mjtByte; // used for true/false - - - -#endif // MUJOCO_INCLUDE_MJTNUM_H_ +#endif // MUJOCO_INCLUDE_MJTNUM_H_ diff --git a/mujoco/include/mujoco/mjvisualize.h b/mujoco/include/mujoco/mjvisualize.h index dc8a4bc6..22774af8 100644 --- a/mujoco/include/mujoco/mjvisualize.h +++ b/mujoco/include/mujoco/mjvisualize.h @@ -19,391 +19,384 @@ #include #include - -#define mjNGROUP 6 // number of geom, site, joint, skin groups with visflags -#define mjMAXLIGHT 100 // maximum number of lights in a scene -#define mjMAXOVERLAY 500 // maximum number of characters in overlay text -#define mjMAXLINE 100 // maximum number of lines per plot -#define mjMAXLINEPNT 1000 // maximum number points per line -#define mjMAXPLANEGRID 200 // maximum number of grid divisions for plane - - -//---------------------------------- primitive types (mjt) ----------------------------------------- - -typedef enum mjtCatBit_ { // bitflags for mjvGeom category - mjCAT_STATIC = 1, // model elements in body 0 - mjCAT_DYNAMIC = 2, // model elements in all other bodies - mjCAT_DECOR = 4, // decorative geoms - mjCAT_ALL = 7 // select all categories +#define mjNGROUP 6 // number of geom, site, joint, skin groups with visflags +#define mjMAXLIGHT 100 // maximum number of lights in a scene +#define mjMAXOVERLAY 500 // maximum number of characters in overlay text +#define mjMAXLINE 100 // maximum number of lines per plot +#define mjMAXLINEPNT 1000 // maximum number points per line +#define mjMAXPLANEGRID 200 // maximum number of grid divisions for plane + +//---------------------------------- primitive types (mjt) +//----------------------------------------- + +typedef enum mjtCatBit_ { // bitflags for mjvGeom category + mjCAT_STATIC = 1, // model elements in body 0 + mjCAT_DYNAMIC = 2, // model elements in all other bodies + mjCAT_DECOR = 4, // decorative geoms + mjCAT_ALL = 7 // select all categories } mjtCatBit; - -typedef enum mjtMouse_ { // mouse interaction mode - mjMOUSE_NONE = 0, // no action - mjMOUSE_ROTATE_V, // rotate, vertical plane - mjMOUSE_ROTATE_H, // rotate, horizontal plane - mjMOUSE_MOVE_V, // move, vertical plane - mjMOUSE_MOVE_H, // move, horizontal plane - mjMOUSE_ZOOM, // zoom - mjMOUSE_SELECT // selection +typedef enum mjtMouse_ { // mouse interaction mode + mjMOUSE_NONE = 0, // no action + mjMOUSE_ROTATE_V, // rotate, vertical plane + mjMOUSE_ROTATE_H, // rotate, horizontal plane + mjMOUSE_MOVE_V, // move, vertical plane + mjMOUSE_MOVE_H, // move, horizontal plane + mjMOUSE_ZOOM, // zoom + mjMOUSE_SELECT // selection } mjtMouse; - -typedef enum mjtPertBit_ { // mouse perturbations - mjPERT_TRANSLATE = 1, // translation - mjPERT_ROTATE = 2 // rotation +typedef enum mjtPertBit_ { // mouse perturbations + mjPERT_TRANSLATE = 1, // translation + mjPERT_ROTATE = 2 // rotation } mjtPertBit; - -typedef enum mjtCamera_ { // abstract camera type - mjCAMERA_FREE = 0, // free camera - mjCAMERA_TRACKING, // tracking camera; uses trackbodyid - mjCAMERA_FIXED, // fixed camera; uses fixedcamid - mjCAMERA_USER // user is responsible for setting OpenGL camera +typedef enum mjtCamera_ { // abstract camera type + mjCAMERA_FREE = 0, // free camera + mjCAMERA_TRACKING, // tracking camera; uses trackbodyid + mjCAMERA_FIXED, // fixed camera; uses fixedcamid + mjCAMERA_USER // user is responsible for setting OpenGL camera } mjtCamera; - -typedef enum mjtLabel_ { // object labeling - mjLABEL_NONE = 0, // nothing - mjLABEL_BODY, // body labels - mjLABEL_JOINT, // joint labels - mjLABEL_GEOM, // geom labels - mjLABEL_SITE, // site labels - mjLABEL_CAMERA, // camera labels - mjLABEL_LIGHT, // light labels - mjLABEL_TENDON, // tendon labels - mjLABEL_ACTUATOR, // actuator labels - mjLABEL_CONSTRAINT, // constraint labels - mjLABEL_FLEX, // flex labels - mjLABEL_SKIN, // skin labels - mjLABEL_SELECTION, // selected object - mjLABEL_SELPNT, // coordinates of selection point - mjLABEL_CONTACTPOINT, // contact information - mjLABEL_CONTACTFORCE, // magnitude of contact force - mjLABEL_ISLAND, // id of island - - mjNLABEL // number of label types +typedef enum mjtLabel_ { // object labeling + mjLABEL_NONE = 0, // nothing + mjLABEL_BODY, // body labels + mjLABEL_JOINT, // joint labels + mjLABEL_GEOM, // geom labels + mjLABEL_SITE, // site labels + mjLABEL_CAMERA, // camera labels + mjLABEL_LIGHT, // light labels + mjLABEL_TENDON, // tendon labels + mjLABEL_ACTUATOR, // actuator labels + mjLABEL_CONSTRAINT, // constraint labels + mjLABEL_FLEX, // flex labels + mjLABEL_SKIN, // skin labels + mjLABEL_SELECTION, // selected object + mjLABEL_SELPNT, // coordinates of selection point + mjLABEL_CONTACTPOINT, // contact information + mjLABEL_CONTACTFORCE, // magnitude of contact force + mjLABEL_ISLAND, // id of island + + mjNLABEL // number of label types } mjtLabel; - -typedef enum mjtFrame_ { // frame visualization - mjFRAME_NONE = 0, // no frames - mjFRAME_BODY, // body frames - mjFRAME_GEOM, // geom frames - mjFRAME_SITE, // site frames - mjFRAME_CAMERA, // camera frames - mjFRAME_LIGHT, // light frames - mjFRAME_CONTACT, // contact frames - mjFRAME_WORLD, // world frame - - mjNFRAME // number of visualization frames +typedef enum mjtFrame_ { // frame visualization + mjFRAME_NONE = 0, // no frames + mjFRAME_BODY, // body frames + mjFRAME_GEOM, // geom frames + mjFRAME_SITE, // site frames + mjFRAME_CAMERA, // camera frames + mjFRAME_LIGHT, // light frames + mjFRAME_CONTACT, // contact frames + mjFRAME_WORLD, // world frame + + mjNFRAME // number of visualization frames } mjtFrame; - -typedef enum mjtVisFlag_ { // flags enabling model element visualization - mjVIS_CONVEXHULL = 0, // mesh convex hull - mjVIS_TEXTURE, // textures - mjVIS_JOINT, // joints - mjVIS_CAMERA, // cameras - mjVIS_ACTUATOR, // actuators - mjVIS_ACTIVATION, // activations - mjVIS_LIGHT, // lights - mjVIS_TENDON, // tendons - mjVIS_RANGEFINDER, // rangefinder sensors - mjVIS_CONSTRAINT, // point constraints - mjVIS_INERTIA, // equivalent inertia boxes - mjVIS_SCLINERTIA, // scale equivalent inertia boxes with mass - mjVIS_PERTFORCE, // perturbation force - mjVIS_PERTOBJ, // perturbation object - mjVIS_CONTACTPOINT, // contact points - mjVIS_ISLAND, // constraint islands - mjVIS_CONTACTFORCE, // contact force - mjVIS_CONTACTSPLIT, // split contact force into normal and tangent - mjVIS_TRANSPARENT, // make dynamic geoms more transparent - mjVIS_AUTOCONNECT, // auto connect joints and body coms - mjVIS_COM, // center of mass - mjVIS_SELECT, // selection point - mjVIS_STATIC, // static bodies - mjVIS_SKIN, // skin - mjVIS_FLEXVERT, // flex vertices - mjVIS_FLEXEDGE, // flex edges - mjVIS_FLEXFACE, // flex element faces - mjVIS_FLEXSKIN, // flex smooth skin (disables the rest) - mjVIS_BODYBVH, // body bounding volume hierarchy - mjVIS_FLEXBVH, // flex bounding volume hierarchy - mjVIS_MESHBVH, // mesh bounding volume hierarchy - mjVIS_SDFITER, // iterations of SDF gradient descent - - mjNVISFLAG // number of visualization flags +typedef enum mjtVisFlag_ { // flags enabling model element visualization + mjVIS_CONVEXHULL = 0, // mesh convex hull + mjVIS_TEXTURE, // textures + mjVIS_JOINT, // joints + mjVIS_CAMERA, // cameras + mjVIS_ACTUATOR, // actuators + mjVIS_ACTIVATION, // activations + mjVIS_LIGHT, // lights + mjVIS_TENDON, // tendons + mjVIS_RANGEFINDER, // rangefinder sensors + mjVIS_CONSTRAINT, // point constraints + mjVIS_INERTIA, // equivalent inertia boxes + mjVIS_SCLINERTIA, // scale equivalent inertia boxes with mass + mjVIS_PERTFORCE, // perturbation force + mjVIS_PERTOBJ, // perturbation object + mjVIS_CONTACTPOINT, // contact points + mjVIS_ISLAND, // constraint islands + mjVIS_CONTACTFORCE, // contact force + mjVIS_CONTACTSPLIT, // split contact force into normal and tangent + mjVIS_TRANSPARENT, // make dynamic geoms more transparent + mjVIS_AUTOCONNECT, // auto connect joints and body coms + mjVIS_COM, // center of mass + mjVIS_SELECT, // selection point + mjVIS_STATIC, // static bodies + mjVIS_SKIN, // skin + mjVIS_FLEXVERT, // flex vertices + mjVIS_FLEXEDGE, // flex edges + mjVIS_FLEXFACE, // flex element faces + mjVIS_FLEXSKIN, // flex smooth skin (disables the rest) + mjVIS_BODYBVH, // body bounding volume hierarchy + mjVIS_FLEXBVH, // flex bounding volume hierarchy + mjVIS_MESHBVH, // mesh bounding volume hierarchy + mjVIS_SDFITER, // iterations of SDF gradient descent + + mjNVISFLAG // number of visualization flags } mjtVisFlag; - -typedef enum mjtRndFlag_ { // flags enabling rendering effects - mjRND_SHADOW = 0, // shadows - mjRND_WIREFRAME, // wireframe - mjRND_REFLECTION, // reflections - mjRND_ADDITIVE, // additive transparency - mjRND_SKYBOX, // skybox - mjRND_FOG, // fog - mjRND_HAZE, // haze - mjRND_SEGMENT, // segmentation with random color - mjRND_IDCOLOR, // segmentation with segid+1 color - mjRND_CULL_FACE, // cull backward faces - - mjNRNDFLAG // number of rendering flags +typedef enum mjtRndFlag_ { // flags enabling rendering effects + mjRND_SHADOW = 0, // shadows + mjRND_WIREFRAME, // wireframe + mjRND_REFLECTION, // reflections + mjRND_ADDITIVE, // additive transparency + mjRND_SKYBOX, // skybox + mjRND_FOG, // fog + mjRND_HAZE, // haze + mjRND_SEGMENT, // segmentation with random color + mjRND_IDCOLOR, // segmentation with segid+1 color + mjRND_CULL_FACE, // cull backward faces + + mjNRNDFLAG // number of rendering flags } mjtRndFlag; - -typedef enum mjtStereo_ { // type of stereo rendering - mjSTEREO_NONE = 0, // no stereo; use left eye only - mjSTEREO_QUADBUFFERED, // quad buffered; revert to side-by-side if no hardware support - mjSTEREO_SIDEBYSIDE // side-by-side +typedef enum mjtStereo_ { // type of stereo rendering + mjSTEREO_NONE = 0, // no stereo; use left eye only + mjSTEREO_QUADBUFFERED, // quad buffered; revert to side-by-side if no hardware + // support + mjSTEREO_SIDEBYSIDE // side-by-side } mjtStereo; - -//---------------------------------- mjvPerturb ---------------------------------------------------- - -struct mjvPerturb_ { // object selection and perturbation - int select; // selected body id; non-positive: none - int flexselect; // selected flex id; negative: none - int skinselect; // selected skin id; negative: none - int active; // perturbation bitmask (mjtPertBit) - int active2; // secondary perturbation bitmask (mjtPertBit) - mjtNum refpos[3]; // reference position for selected object - mjtNum refquat[4]; // reference orientation for selected object - mjtNum refselpos[3]; // reference position for selection point - mjtNum localpos[3]; // selection point in object coordinates - mjtNum localmass; // spatial inertia at selection point - mjtNum scale; // relative mouse motion-to-space scaling (set by initPerturb) +//---------------------------------- mjvPerturb +//---------------------------------------------------- + +struct mjvPerturb_ { // object selection and perturbation + int select; // selected body id; non-positive: none + int flexselect; // selected flex id; negative: none + int skinselect; // selected skin id; negative: none + int active; // perturbation bitmask (mjtPertBit) + int active2; // secondary perturbation bitmask (mjtPertBit) + mjtNum refpos[3]; // reference position for selected object + mjtNum refquat[4]; // reference orientation for selected object + mjtNum refselpos[3]; // reference position for selection point + mjtNum localpos[3]; // selection point in object coordinates + mjtNum localmass; // spatial inertia at selection point + mjtNum scale; // relative mouse motion-to-space scaling (set by initPerturb) }; typedef struct mjvPerturb_ mjvPerturb; +//---------------------------------- mjvCamera +//----------------------------------------------------- -//---------------------------------- mjvCamera ----------------------------------------------------- - -struct mjvCamera_ { // abstract camera +struct mjvCamera_ { // abstract camera // type and ids - int type; // camera type (mjtCamera) - int fixedcamid; // fixed camera id - int trackbodyid; // body id to track + int type; // camera type (mjtCamera) + int fixedcamid; // fixed camera id + int trackbodyid; // body id to track // abstract camera pose specification - mjtNum lookat[3]; // lookat point - mjtNum distance; // distance to lookat point or tracked body - mjtNum azimuth; // camera azimuth (deg) - mjtNum elevation; // camera elevation (deg) + mjtNum lookat[3]; // lookat point + mjtNum distance; // distance to lookat point or tracked body + mjtNum azimuth; // camera azimuth (deg) + mjtNum elevation; // camera elevation (deg) }; typedef struct mjvCamera_ mjvCamera; +//---------------------------------- mjvGLCamera +//--------------------------------------------------- -//---------------------------------- mjvGLCamera --------------------------------------------------- - -struct mjvGLCamera_ { // OpenGL camera +struct mjvGLCamera_ { // OpenGL camera // camera frame - float pos[3]; // position - float forward[3]; // forward direction - float up[3]; // up direction + float pos[3]; // position + float forward[3]; // forward direction + float up[3]; // up direction // camera projection - float frustum_center; // hor. center (left,right set to match aspect) - float frustum_width; // width (not used for rendering) - float frustum_bottom; // bottom - float frustum_top; // top - float frustum_near; // near - float frustum_far; // far + float frustum_center; // hor. center (left,right set to match aspect) + float frustum_width; // width (not used for rendering) + float frustum_bottom; // bottom + float frustum_top; // top + float frustum_near; // near + float frustum_far; // far }; typedef struct mjvGLCamera_ mjvGLCamera; +//---------------------------------- mjvGeom +//------------------------------------------------------- -//---------------------------------- mjvGeom ------------------------------------------------------- - -struct mjvGeom_ { // abstract geom +struct mjvGeom_ { // abstract geom // type info - int type; // geom type (mjtGeom) - int dataid; // mesh, hfield or plane id; -1: none - int objtype; // mujoco object type; mjOBJ_UNKNOWN for decor - int objid; // mujoco object id; -1 for decor - int category; // visual category - int texid; // texture id; -1: no texture - int texuniform; // uniform cube mapping - int texcoord; // mesh or flex geom has texture coordinates - int segid; // segmentation id; -1: not shown + int type; // geom type (mjtGeom) + int dataid; // mesh, hfield or plane id; -1: none + int objtype; // mujoco object type; mjOBJ_UNKNOWN for decor + int objid; // mujoco object id; -1 for decor + int category; // visual category + int texid; // texture id; -1: no texture + int texuniform; // uniform cube mapping + int texcoord; // mesh or flex geom has texture coordinates + int segid; // segmentation id; -1: not shown // OpenGL info - float texrepeat[2]; // texture repetition for 2D mapping - float size[3]; // size parameters - float pos[3]; // Cartesian position - float mat[9]; // Cartesian orientation - float rgba[4]; // color and transparency - float emission; // emission coef - float specular; // specular coef - float shininess; // shininess coef - float reflectance; // reflectance coef - char label[100]; // text label + float texrepeat[2]; // texture repetition for 2D mapping + float size[3]; // size parameters + float pos[3]; // Cartesian position + float mat[9]; // Cartesian orientation + float rgba[4]; // color and transparency + float emission; // emission coef + float specular; // specular coef + float shininess; // shininess coef + float reflectance; // reflectance coef + char label[100]; // text label // transparency rendering (set internally) - float camdist; // distance to camera (used by sorter) - float modelrbound; // geom rbound from model, 0 if not model geom - mjtByte transparent; // treat geom as transparent + float camdist; // distance to camera (used by sorter) + float modelrbound; // geom rbound from model, 0 if not model geom + mjtByte transparent; // treat geom as transparent }; typedef struct mjvGeom_ mjvGeom; - -//---------------------------------- mjvLight ------------------------------------------------------ - -struct mjvLight_ { // OpenGL light - float pos[3]; // position rel. to body frame - float dir[3]; // direction rel. to body frame - float attenuation[3]; // OpenGL attenuation (quadratic model) - float cutoff; // OpenGL cutoff - float exponent; // OpenGL exponent - float ambient[3]; // ambient rgb (alpha=1) - float diffuse[3]; // diffuse rgb (alpha=1) - float specular[3]; // specular rgb (alpha=1) - mjtByte headlight; // headlight - mjtByte directional; // directional light - mjtByte castshadow; // does light cast shadows +//---------------------------------- mjvLight +//------------------------------------------------------ + +struct mjvLight_ { // OpenGL light + float pos[3]; // position rel. to body frame + float dir[3]; // direction rel. to body frame + float attenuation[3]; // OpenGL attenuation (quadratic model) + float cutoff; // OpenGL cutoff + float exponent; // OpenGL exponent + float ambient[3]; // ambient rgb (alpha=1) + float diffuse[3]; // diffuse rgb (alpha=1) + float specular[3]; // specular rgb (alpha=1) + mjtByte headlight; // headlight + mjtByte directional; // directional light + mjtByte castshadow; // does light cast shadows }; typedef struct mjvLight_ mjvLight; - -//---------------------------------- mjvOption ----------------------------------------------------- - -struct mjvOption_ { // abstract visualization options - int label; // what objects to label (mjtLabel) - int frame; // which frame to show (mjtFrame) - mjtByte geomgroup[mjNGROUP]; // geom visualization by group - mjtByte sitegroup[mjNGROUP]; // site visualization by group - mjtByte jointgroup[mjNGROUP]; // joint visualization by group - mjtByte tendongroup[mjNGROUP]; // tendon visualization by group - mjtByte actuatorgroup[mjNGROUP]; // actuator visualization by group - mjtByte flexgroup[mjNGROUP]; // flex visualization by group - mjtByte skingroup[mjNGROUP]; // skin visualization by group - mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag) - int bvh_depth; // depth of the bounding volume hierarchy to be visualized - int flex_layer; // element layer to be visualized for 3D flex +//---------------------------------- mjvOption +//----------------------------------------------------- + +struct mjvOption_ { // abstract visualization options + int label; // what objects to label (mjtLabel) + int frame; // which frame to show (mjtFrame) + mjtByte geomgroup[mjNGROUP]; // geom visualization by group + mjtByte sitegroup[mjNGROUP]; // site visualization by group + mjtByte jointgroup[mjNGROUP]; // joint visualization by group + mjtByte tendongroup[mjNGROUP]; // tendon visualization by group + mjtByte actuatorgroup[mjNGROUP]; // actuator visualization by group + mjtByte flexgroup[mjNGROUP]; // flex visualization by group + mjtByte skingroup[mjNGROUP]; // skin visualization by group + mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag) + int bvh_depth; // depth of the bounding volume hierarchy to be visualized + int flex_layer; // element layer to be visualized for 3D flex }; typedef struct mjvOption_ mjvOption; +//---------------------------------- mjvScene +//------------------------------------------------------ -//---------------------------------- mjvScene ------------------------------------------------------ - -struct mjvScene_ { // abstract scene passed to OpenGL renderer +struct mjvScene_ { // abstract scene passed to OpenGL renderer // abstract geoms - int maxgeom; // size of allocated geom buffer - int ngeom; // number of geoms currently in buffer - mjvGeom* geoms; // buffer for geoms (ngeom) - int* geomorder; // buffer for ordering geoms by distance to camera (ngeom) + int maxgeom; // size of allocated geom buffer + int ngeom; // number of geoms currently in buffer + mjvGeom *geoms; // buffer for geoms (ngeom) + int *geomorder; // buffer for ordering geoms by distance to camera (ngeom) // flex data - int nflex; // number of flexes - int* flexedgeadr; // address of flex edges (nflex) - int* flexedgenum; // number of edges in flex (nflex) - int* flexvertadr; // address of flex vertices (nflex) - int* flexvertnum; // number of vertices in flex (nflex) - int* flexfaceadr; // address of flex faces (nflex) - int* flexfacenum; // number of flex faces allocated (nflex) - int* flexfaceused; // number of flex faces currently in use (nflex) - int* flexedge; // flex edge data (2*nflexedge) - float* flexvert; // flex vertices (3*nflexvert) - float* flexface; // flex faces vertices (9*sum(flexfacenum)) - float* flexnormal; // flex face normals (9*sum(flexfacenum)) - float* flextexcoord; // flex face texture coordinates (6*sum(flexfacenum)) - mjtByte flexvertopt; // copy of mjVIS_FLEXVERT mjvOption flag - mjtByte flexedgeopt; // copy of mjVIS_FLEXEDGE mjvOption flag - mjtByte flexfaceopt; // copy of mjVIS_FLEXFACE mjvOption flag - mjtByte flexskinopt; // copy of mjVIS_FLEXSKIN mjvOption flag + int nflex; // number of flexes + int *flexedgeadr; // address of flex edges (nflex) + int *flexedgenum; // number of edges in flex (nflex) + int *flexvertadr; // address of flex vertices (nflex) + int *flexvertnum; // number of vertices in flex (nflex) + int *flexfaceadr; // address of flex faces (nflex) + int *flexfacenum; // number of flex faces allocated (nflex) + int *flexfaceused; // number of flex faces currently in use (nflex) + int *flexedge; // flex edge data (2*nflexedge) + float *flexvert; // flex vertices (3*nflexvert) + float *flexface; // flex faces vertices (9*sum(flexfacenum)) + float *flexnormal; // flex face normals (9*sum(flexfacenum)) + float *flextexcoord; // flex face texture coordinates (6*sum(flexfacenum)) + mjtByte flexvertopt; // copy of mjVIS_FLEXVERT mjvOption flag + mjtByte flexedgeopt; // copy of mjVIS_FLEXEDGE mjvOption flag + mjtByte flexfaceopt; // copy of mjVIS_FLEXFACE mjvOption flag + mjtByte flexskinopt; // copy of mjVIS_FLEXSKIN mjvOption flag // skin data - int nskin; // number of skins - int* skinfacenum; // number of faces in skin (nskin) - int* skinvertadr; // address of skin vertices (nskin) - int* skinvertnum; // number of vertices in skin (nskin) - float* skinvert; // skin vertex data (3*nskinvert) - float* skinnormal; // skin normal data (3*nskinvert) + int nskin; // number of skins + int *skinfacenum; // number of faces in skin (nskin) + int *skinvertadr; // address of skin vertices (nskin) + int *skinvertnum; // number of vertices in skin (nskin) + float *skinvert; // skin vertex data (3*nskinvert) + float *skinnormal; // skin normal data (3*nskinvert) // OpenGL lights - int nlight; // number of lights currently in buffer - mjvLight lights[mjMAXLIGHT]; // buffer for lights (nlight) + int nlight; // number of lights currently in buffer + mjvLight lights[mjMAXLIGHT]; // buffer for lights (nlight) // OpenGL cameras - mjvGLCamera camera[2]; // left and right camera + mjvGLCamera camera[2]; // left and right camera // OpenGL model transformation - mjtByte enabletransform; // enable model transformation - float translate[3]; // model translation - float rotate[4]; // model quaternion rotation - float scale; // model scaling + mjtByte enabletransform; // enable model transformation + float translate[3]; // model translation + float rotate[4]; // model quaternion rotation + float scale; // model scaling // OpenGL rendering effects - int stereo; // stereoscopic rendering (mjtStereo) - mjtByte flags[mjNRNDFLAG]; // rendering flags (indexed by mjtRndFlag) + int stereo; // stereoscopic rendering (mjtStereo) + mjtByte flags[mjNRNDFLAG]; // rendering flags (indexed by mjtRndFlag) // framing - int framewidth; // frame pixel width; 0: disable framing - float framergb[3]; // frame color + int framewidth; // frame pixel width; 0: disable framing + float framergb[3]; // frame color }; typedef struct mjvScene_ mjvScene; +//---------------------------------- mjvFigure +//----------------------------------------------------- -//---------------------------------- mjvFigure ----------------------------------------------------- - -struct mjvFigure_ { // abstract 2D figure passed to OpenGL renderer +struct mjvFigure_ { // abstract 2D figure passed to OpenGL renderer // enable flags - int flg_legend; // show legend - int flg_ticklabel[2]; // show grid tick labels (x,y) - int flg_extend; // automatically extend axis ranges to fit data - int flg_barplot; // isolated line segments (i.e. GL_LINES) - int flg_selection; // vertical selection line - int flg_symmetric; // symmetric y-axis + int flg_legend; // show legend + int flg_ticklabel[2]; // show grid tick labels (x,y) + int flg_extend; // automatically extend axis ranges to fit data + int flg_barplot; // isolated line segments (i.e. GL_LINES) + int flg_selection; // vertical selection line + int flg_symmetric; // symmetric y-axis // style settings - float linewidth; // line width - float gridwidth; // grid line width - int gridsize[2]; // number of grid points in (x,y) - float gridrgb[3]; // grid line rgb - float figurergba[4]; // figure color and alpha - float panergba[4]; // pane color and alpha - float legendrgba[4]; // legend color and alpha - float textrgb[3]; // text color - float linergb[mjMAXLINE][3]; // line colors - float range[2][2]; // axis ranges; (min>=max) automatic - char xformat[20]; // x-tick label format for sprintf - char yformat[20]; // y-tick label format for sprintf - char minwidth[20]; // string used to determine min y-tick width + float linewidth; // line width + float gridwidth; // grid line width + int gridsize[2]; // number of grid points in (x,y) + float gridrgb[3]; // grid line rgb + float figurergba[4]; // figure color and alpha + float panergba[4]; // pane color and alpha + float legendrgba[4]; // legend color and alpha + float textrgb[3]; // text color + float linergb[mjMAXLINE][3]; // line colors + float range[2][2]; // axis ranges; (min>=max) automatic + char xformat[20]; // x-tick label format for sprintf + char yformat[20]; // y-tick label format for sprintf + char minwidth[20]; // string used to determine min y-tick width // text labels - char title[1000]; // figure title; subplots separated with 2+ spaces - char xlabel[100]; // x-axis label - char linename[mjMAXLINE][100]; // line names for legend + char title[1000]; // figure title; subplots separated with 2+ spaces + char xlabel[100]; // x-axis label + char linename[mjMAXLINE][100]; // line names for legend // dynamic settings - int legendoffset; // number of lines to offset legend - int subplot; // selected subplot (for title rendering) - int highlight[2]; // if point is in legend rect, highlight line - int highlightid; // if id>=0 and no point, highlight id - float selection; // selection line x-value + int legendoffset; // number of lines to offset legend + int subplot; // selected subplot (for title rendering) + int highlight[2]; // if point is in legend rect, highlight line + int highlightid; // if id>=0 and no point, highlight id + float selection; // selection line x-value // line data - int linepnt[mjMAXLINE]; // number of points in line; (0) disable - float linedata[mjMAXLINE][2*mjMAXLINEPNT]; // line data (x,y) + int linepnt[mjMAXLINE]; // number of points in line; (0) disable + float linedata[mjMAXLINE][2 * mjMAXLINEPNT]; // line data (x,y) // output from renderer - int xaxispixel[2]; // range of x-axis in pixels - int yaxispixel[2]; // range of y-axis in pixels - float xaxisdata[2]; // range of x-axis in data units - float yaxisdata[2]; // range of y-axis in data units + int xaxispixel[2]; // range of x-axis in pixels + int yaxispixel[2]; // range of y-axis in pixels + float xaxisdata[2]; // range of x-axis in data units + float yaxisdata[2]; // range of y-axis in data units }; typedef struct mjvFigure_ mjvFigure; - -//---------------------------------- mjvSceneState ------------------------------------------------- +//---------------------------------- mjvSceneState +//------------------------------------------------- struct mjvSceneState_ { - int nbuffer; // size of the buffer in bytes - void* buffer; // heap-allocated memory for all arrays in this struct - int maxgeom; // maximum number of mjvGeom supported by this state object - mjvScene scratch; // scratch space for vis geoms inserted by the user and plugins + int nbuffer; // size of the buffer in bytes + void *buffer; // heap-allocated memory for all arrays in this struct + int maxgeom; // maximum number of mjvGeom supported by this state object + mjvScene + scratch; // scratch space for vis geoms inserted by the user and plugins // fields in mjModel that are necessary to re-render a scene struct { @@ -442,170 +435,170 @@ struct mjvSceneState_ { mjVisual vis; mjStatistic stat; - int* body_parentid; - int* body_rootid; - int* body_weldid; - int* body_mocapid; - int* body_jntnum; - int* body_jntadr; - int* body_dofnum; - int* body_dofadr; - int* body_geomnum; - int* body_geomadr; - mjtNum* body_iquat; - mjtNum* body_mass; - mjtNum* body_inertia; - int* body_bvhadr; - int* body_bvhnum; - - int* bvh_depth; - int* bvh_child; - int* bvh_nodeid; - mjtNum* bvh_aabb; - - int* jnt_type; - int* jnt_bodyid; - int* jnt_group; - - int* geom_type; - int* geom_bodyid; - int* geom_contype; - int* geom_conaffinity; - int* geom_dataid; - int* geom_matid; - int* geom_group; - mjtNum* geom_size; - mjtNum* geom_aabb; - mjtNum* geom_rbound; - float* geom_rgba; - - int* site_type; - int* site_bodyid; - int* site_matid; - int* site_group; - mjtNum* site_size; - float* site_rgba; - - mjtNum* cam_fovy; - mjtNum* cam_ipd; - float* cam_intrinsic; - float* cam_sensorsize; - - mjtByte* light_directional; - mjtByte* light_castshadow; - mjtByte* light_active; - float* light_attenuation; - float* light_cutoff; - float* light_exponent; - float* light_ambient; - float* light_diffuse; - float* light_specular; - - mjtByte* flex_flatskin; - int* flex_dim; - int* flex_matid; - int* flex_group; - int* flex_vertadr; - int* flex_vertnum; - int* flex_elem; - int* flex_elemlayer; - int* flex_elemadr; - int* flex_elemnum; - int* flex_elemdataadr; - int* flex_shell; - int* flex_shellnum; - int* flex_shelldataadr; - int* flex_texcoordadr; - int* flex_bvhadr; - int* flex_bvhnum; - mjtNum* flex_radius; - float* flex_rgba; - - int* hfield_pathadr; - - int* mesh_bvhadr; - int* mesh_bvhnum; - int* mesh_texcoordadr; - int* mesh_graphadr; - int* mesh_pathadr; - - int* skin_matid; - int* skin_group; - float* skin_rgba; - float* skin_inflate; - int* skin_vertadr; - int* skin_vertnum; - int* skin_texcoordadr; - int* skin_faceadr; - int* skin_facenum; - int* skin_boneadr; - int* skin_bonenum; - float* skin_vert; - int* skin_face; - int* skin_bonevertadr; - int* skin_bonevertnum; - float* skin_bonebindpos; - float* skin_bonebindquat; - int* skin_bonebodyid; - int* skin_bonevertid; - float* skin_bonevertweight; - int* skin_pathadr; - - int* tex_pathadr; - - int* mat_texid; - mjtByte* mat_texuniform; - float* mat_texrepeat; - float* mat_emission; - float* mat_specular; - float* mat_shininess; - float* mat_reflectance; - float* mat_rgba; - - int* eq_type; - int* eq_obj1id; - int* eq_obj2id; - mjtNum* eq_data; - - int* tendon_num; - int* tendon_matid; - int* tendon_group; - mjtByte* tendon_limited; - mjtNum* tendon_width; - mjtNum* tendon_range; - mjtNum* tendon_stiffness; - mjtNum* tendon_damping; - mjtNum* tendon_frictionloss; - mjtNum* tendon_lengthspring; - float* tendon_rgba; - - int* actuator_trntype; - int* actuator_dyntype; - int* actuator_trnid; - int* actuator_actadr; - int* actuator_actnum; - int* actuator_group; - mjtByte* actuator_ctrllimited; - mjtByte* actuator_actlimited; - mjtNum* actuator_ctrlrange; - mjtNum* actuator_actrange; - mjtNum* actuator_cranklength; - - int* sensor_type; - int* sensor_objid; - int* sensor_adr; - - int* name_bodyadr; - int* name_jntadr; - int* name_geomadr; - int* name_siteadr; - int* name_camadr; - int* name_lightadr; - int* name_eqadr; - int* name_tendonadr; - int* name_actuatoradr; - char* names; - char* paths; + int *body_parentid; + int *body_rootid; + int *body_weldid; + int *body_mocapid; + int *body_jntnum; + int *body_jntadr; + int *body_dofnum; + int *body_dofadr; + int *body_geomnum; + int *body_geomadr; + mjtNum *body_iquat; + mjtNum *body_mass; + mjtNum *body_inertia; + int *body_bvhadr; + int *body_bvhnum; + + int *bvh_depth; + int *bvh_child; + int *bvh_nodeid; + mjtNum *bvh_aabb; + + int *jnt_type; + int *jnt_bodyid; + int *jnt_group; + + int *geom_type; + int *geom_bodyid; + int *geom_contype; + int *geom_conaffinity; + int *geom_dataid; + int *geom_matid; + int *geom_group; + mjtNum *geom_size; + mjtNum *geom_aabb; + mjtNum *geom_rbound; + float *geom_rgba; + + int *site_type; + int *site_bodyid; + int *site_matid; + int *site_group; + mjtNum *site_size; + float *site_rgba; + + mjtNum *cam_fovy; + mjtNum *cam_ipd; + float *cam_intrinsic; + float *cam_sensorsize; + + mjtByte *light_directional; + mjtByte *light_castshadow; + mjtByte *light_active; + float *light_attenuation; + float *light_cutoff; + float *light_exponent; + float *light_ambient; + float *light_diffuse; + float *light_specular; + + mjtByte *flex_flatskin; + int *flex_dim; + int *flex_matid; + int *flex_group; + int *flex_vertadr; + int *flex_vertnum; + int *flex_elem; + int *flex_elemlayer; + int *flex_elemadr; + int *flex_elemnum; + int *flex_elemdataadr; + int *flex_shell; + int *flex_shellnum; + int *flex_shelldataadr; + int *flex_texcoordadr; + int *flex_bvhadr; + int *flex_bvhnum; + mjtNum *flex_radius; + float *flex_rgba; + + int *hfield_pathadr; + + int *mesh_bvhadr; + int *mesh_bvhnum; + int *mesh_texcoordadr; + int *mesh_graphadr; + int *mesh_pathadr; + + int *skin_matid; + int *skin_group; + float *skin_rgba; + float *skin_inflate; + int *skin_vertadr; + int *skin_vertnum; + int *skin_texcoordadr; + int *skin_faceadr; + int *skin_facenum; + int *skin_boneadr; + int *skin_bonenum; + float *skin_vert; + int *skin_face; + int *skin_bonevertadr; + int *skin_bonevertnum; + float *skin_bonebindpos; + float *skin_bonebindquat; + int *skin_bonebodyid; + int *skin_bonevertid; + float *skin_bonevertweight; + int *skin_pathadr; + + int *tex_pathadr; + + int *mat_texid; + mjtByte *mat_texuniform; + float *mat_texrepeat; + float *mat_emission; + float *mat_specular; + float *mat_shininess; + float *mat_reflectance; + float *mat_rgba; + + int *eq_type; + int *eq_obj1id; + int *eq_obj2id; + mjtNum *eq_data; + + int *tendon_num; + int *tendon_matid; + int *tendon_group; + mjtByte *tendon_limited; + mjtNum *tendon_width; + mjtNum *tendon_range; + mjtNum *tendon_stiffness; + mjtNum *tendon_damping; + mjtNum *tendon_frictionloss; + mjtNum *tendon_lengthspring; + float *tendon_rgba; + + int *actuator_trntype; + int *actuator_dyntype; + int *actuator_trnid; + int *actuator_actadr; + int *actuator_actnum; + int *actuator_group; + mjtByte *actuator_ctrllimited; + mjtByte *actuator_actlimited; + mjtNum *actuator_ctrlrange; + mjtNum *actuator_actrange; + mjtNum *actuator_cranklength; + + int *sensor_type; + int *sensor_objid; + int *sensor_adr; + + int *name_bodyadr; + int *name_jntadr; + int *name_geomadr; + int *name_siteadr; + int *name_camadr; + int *name_lightadr; + int *name_eqadr; + int *name_tendonadr; + int *name_actuatoradr; + char *names; + char *paths; } model; // fields in mjData that are necessary to re-render a scene @@ -618,53 +611,53 @@ struct mjvSceneState_ { mjtNum time; - mjtNum* act; - - mjtNum* ctrl; - mjtNum* xfrc_applied; - mjtByte* eq_active; - - mjtNum* sensordata; - - mjtNum* xpos; - mjtNum* xquat; - mjtNum* xmat; - mjtNum* xipos; - mjtNum* ximat; - mjtNum* xanchor; - mjtNum* xaxis; - mjtNum* geom_xpos; - mjtNum* geom_xmat; - mjtNum* site_xpos; - mjtNum* site_xmat; - mjtNum* cam_xpos; - mjtNum* cam_xmat; - mjtNum* light_xpos; - mjtNum* light_xdir; - - mjtNum* subtree_com; - - int* ten_wrapadr; - int* ten_wrapnum; - int* wrap_obj; - mjtNum* ten_length; - mjtNum* wrap_xpos; - - mjtNum* bvh_aabb_dyn; - mjtByte* bvh_active; - int* island_dofadr; - int* island_dofind; - int* dof_island; - int* efc_island; - int* tendon_efcadr; - - mjtNum* flexvert_xpos; - - mjContact* contact; - mjtNum* efc_force; - void* arena; + mjtNum *act; + + mjtNum *ctrl; + mjtNum *xfrc_applied; + mjtByte *eq_active; + + mjtNum *sensordata; + + mjtNum *xpos; + mjtNum *xquat; + mjtNum *xmat; + mjtNum *xipos; + mjtNum *ximat; + mjtNum *xanchor; + mjtNum *xaxis; + mjtNum *geom_xpos; + mjtNum *geom_xmat; + mjtNum *site_xpos; + mjtNum *site_xmat; + mjtNum *cam_xpos; + mjtNum *cam_xmat; + mjtNum *light_xpos; + mjtNum *light_xdir; + + mjtNum *subtree_com; + + int *ten_wrapadr; + int *ten_wrapnum; + int *wrap_obj; + mjtNum *ten_length; + mjtNum *wrap_xpos; + + mjtNum *bvh_aabb_dyn; + mjtByte *bvh_active; + int *island_dofadr; + int *island_dofind; + int *dof_island; + int *efc_island; + int *tendon_efcadr; + + mjtNum *flexvert_xpos; + + mjContact *contact; + mjtNum *efc_force; + void *arena; } data; }; typedef struct mjvSceneState_ mjvSceneState; -#endif // MUJOCO_MJVISUALIZE_H_ +#endif // MUJOCO_MJVISUALIZE_H_ diff --git a/mujoco/include/mujoco/mjxmacro.h b/mujoco/include/mujoco/mjxmacro.h index 7cb4fa7b..880a4530 100644 --- a/mujoco/include/mujoco/mjxmacro.h +++ b/mujoco/include/mujoco/mjxmacro.h @@ -15,744 +15,734 @@ #ifndef MUJOCO_MJXMACRO_H_ #define MUJOCO_MJXMACRO_H_ - -//-------------------------------- mjOption -------------------------------------------------------- +//-------------------------------- mjOption +//-------------------------------------------------------- // scalar fields of mjOption -#define MJOPTION_FLOATS \ - X( mjtNum, timestep ) \ - X( mjtNum, apirate ) \ - X( mjtNum, impratio ) \ - X( mjtNum, tolerance ) \ - X( mjtNum, ls_tolerance ) \ - X( mjtNum, noslip_tolerance ) \ - X( mjtNum, mpr_tolerance ) \ - X( mjtNum, density ) \ - X( mjtNum, viscosity ) \ - X( mjtNum, o_margin ) \ - - -#define MJOPTION_INTS \ - X( int, integrator ) \ - X( int, cone ) \ - X( int, jacobian ) \ - X( int, solver ) \ - X( int, iterations ) \ - X( int, ls_iterations ) \ - X( int, noslip_iterations ) \ - X( int, mpr_iterations ) \ - X( int, disableflags ) \ - X( int, enableflags ) \ - X( int, disableactuator ) \ - X( int, sdf_initpoints ) \ - X( int, sdf_iterations ) - - -#define MJOPTION_SCALARS \ - MJOPTION_FLOATS \ - MJOPTION_INTS - +#define MJOPTION_FLOATS \ + X(mjtNum, timestep) \ + X(mjtNum, apirate) \ + X(mjtNum, impratio) \ + X(mjtNum, tolerance) \ + X(mjtNum, ls_tolerance) \ + X(mjtNum, noslip_tolerance) \ + X(mjtNum, mpr_tolerance) \ + X(mjtNum, density) \ + X(mjtNum, viscosity) \ + X(mjtNum, o_margin) + +#define MJOPTION_INTS \ + X(int, integrator) \ + X(int, cone) \ + X(int, jacobian) \ + X(int, solver) \ + X(int, iterations) \ + X(int, ls_iterations) \ + X(int, noslip_iterations) \ + X(int, mpr_iterations) \ + X(int, disableflags) \ + X(int, enableflags) \ + X(int, disableactuator) \ + X(int, sdf_initpoints) \ + X(int, sdf_iterations) + +#define MJOPTION_SCALARS \ + MJOPTION_FLOATS \ + MJOPTION_INTS // vector fields of mjOption -#define MJOPTION_VECTORS \ - X( gravity, 3 ) \ - X( wind, 3 ) \ - X( magnetic, 3 ) \ - X( o_solref, mjNREF ) \ - X( o_solimp, mjNIMP ) \ - X( o_friction, 5 ) - +#define MJOPTION_VECTORS \ + X(gravity, 3) \ + X(wind, 3) \ + X(magnetic, 3) \ + X(o_solref, mjNREF) \ + X(o_solimp, mjNIMP) \ + X(o_friction, 5) -//-------------------------------- mjModel --------------------------------------------------------- +//-------------------------------- mjModel +//--------------------------------------------------------- // int fields of mjModel -#define MJMODEL_INTS \ - X ( nq ) \ - XMJV( nv ) \ - XMJV( nu ) \ - XMJV( na ) \ - XMJV( nbody ) \ - XMJV( nbvh ) \ - XMJV( nbvhstatic ) \ - X ( nbvhdynamic ) \ - XMJV( njnt ) \ - XMJV( ngeom ) \ - XMJV( nsite ) \ - XMJV( ncam ) \ - XMJV( nlight ) \ - XMJV( nflex ) \ - XMJV( nflexvert ) \ - X ( nflexedge ) \ - X ( nflexelem ) \ - X ( nflexelemdata ) \ - X ( nflexshelldata ) \ - X ( nflexevpair ) \ - XMJV( nflextexcoord ) \ - XMJV( nmesh ) \ - X ( nmeshvert ) \ - X ( nmeshnormal ) \ - X ( nmeshtexcoord ) \ - X ( nmeshface ) \ - X ( nmeshgraph ) \ - XMJV( nskin ) \ - XMJV( nskinvert ) \ - X ( nskintexvert ) \ - XMJV( nskinface ) \ - XMJV( nskinbone ) \ - XMJV( nskinbonevert ) \ - X ( nhfield ) \ - X ( nhfielddata ) \ - X ( ntex ) \ - X ( ntexdata ) \ - XMJV( nmat ) \ - X ( npair ) \ - X ( nexclude ) \ - XMJV( neq ) \ - XMJV( ntendon ) \ - XMJV( nwrap ) \ - XMJV( nsensor ) \ - X ( nnumeric ) \ - X ( nnumericdata ) \ - X ( ntext ) \ - X ( ntextdata ) \ - X ( ntuple ) \ - X ( ntupledata ) \ - X ( nkey ) \ - X ( nmocap ) \ - X ( nplugin ) \ - X ( npluginattr ) \ - X ( nuser_body ) \ - X ( nuser_jnt ) \ - X ( nuser_geom ) \ - X ( nuser_site ) \ - X ( nuser_cam ) \ - X ( nuser_tendon ) \ - X ( nuser_actuator ) \ - X ( nuser_sensor ) \ - XMJV( nnames ) \ - XMJV( npaths ) \ - X ( nnames_map ) \ - X ( nM ) \ - X ( nD ) \ - X ( nB ) \ - X ( nemax ) \ - X ( njmax ) \ - X ( nconmax ) \ - XMJV( ntree ) \ - X ( nuserdata ) \ - XMJV( nsensordata ) \ - X ( npluginstate ) \ - X ( narena ) \ - X ( nbuffer ) - - /* nbuffer needs to be the final field */ - - -// define symbols needed in MJMODEL_POINTERS (corresponding to number of columns) -#define MJMODEL_POINTERS_PREAMBLE( m ) \ - int nuser_body = m->nuser_body; \ - int nuser_jnt = m->nuser_jnt; \ - int nuser_geom = m->nuser_geom; \ - int nuser_site = m->nuser_site; \ - int nuser_cam = m->nuser_cam; \ - int nuser_tendon = m->nuser_tendon; \ - int nuser_actuator = m->nuser_actuator; \ - int nuser_sensor = m->nuser_sensor; \ - int nq = m->nq; \ - int nv = m->nv; \ - int na = m->na; \ - int nu = m->nu; \ - int nmocap = m->nmocap; +#define MJMODEL_INTS \ + X(nq) \ + XMJV(nv) \ + XMJV(nu) \ + XMJV(na) \ + XMJV(nbody) \ + XMJV(nbvh) \ + XMJV(nbvhstatic) \ + X(nbvhdynamic) \ + XMJV(njnt) \ + XMJV(ngeom) \ + XMJV(nsite) \ + XMJV(ncam) \ + XMJV(nlight) \ + XMJV(nflex) \ + XMJV(nflexvert) \ + X(nflexedge) \ + X(nflexelem) \ + X(nflexelemdata) \ + X(nflexshelldata) \ + X(nflexevpair) \ + XMJV(nflextexcoord) \ + XMJV(nmesh) \ + X(nmeshvert) \ + X(nmeshnormal) \ + X(nmeshtexcoord) \ + X(nmeshface) \ + X(nmeshgraph) \ + XMJV(nskin) \ + XMJV(nskinvert) \ + X(nskintexvert) \ + XMJV(nskinface) \ + XMJV(nskinbone) \ + XMJV(nskinbonevert) \ + X(nhfield) \ + X(nhfielddata) \ + X(ntex) \ + X(ntexdata) \ + XMJV(nmat) \ + X(npair) \ + X(nexclude) \ + XMJV(neq) \ + XMJV(ntendon) \ + XMJV(nwrap) \ + XMJV(nsensor) \ + X(nnumeric) \ + X(nnumericdata) \ + X(ntext) \ + X(ntextdata) \ + X(ntuple) \ + X(ntupledata) \ + X(nkey) \ + X(nmocap) \ + X(nplugin) \ + X(npluginattr) \ + X(nuser_body) \ + X(nuser_jnt) \ + X(nuser_geom) \ + X(nuser_site) \ + X(nuser_cam) \ + X(nuser_tendon) \ + X(nuser_actuator) \ + X(nuser_sensor) \ + XMJV(nnames) \ + XMJV(npaths) \ + X(nnames_map) \ + X(nM) \ + X(nD) \ + X(nB) \ + X(nemax) \ + X(njmax) \ + X(nconmax) \ + XMJV(ntree) \ + X(nuserdata) \ + XMJV(nsensordata) \ + X(npluginstate) \ + X(narena) \ + X(nbuffer) + +/* nbuffer needs to be the final field */ + +// define symbols needed in MJMODEL_POINTERS (corresponding to number of +// columns) +#define MJMODEL_POINTERS_PREAMBLE(m) \ + int nuser_body = m->nuser_body; \ + int nuser_jnt = m->nuser_jnt; \ + int nuser_geom = m->nuser_geom; \ + int nuser_site = m->nuser_site; \ + int nuser_cam = m->nuser_cam; \ + int nuser_tendon = m->nuser_tendon; \ + int nuser_actuator = m->nuser_actuator; \ + int nuser_sensor = m->nuser_sensor; \ + int nq = m->nq; \ + int nv = m->nv; \ + int na = m->na; \ + int nu = m->nu; \ + int nmocap = m->nmocap; // macro for annotating that an array size in an X macro is a member of mjModel // by default this macro does nothing, but users can redefine it as necessary #define MJ_M(n) n - // pointer fields of mjModel // XMJV means that the field is required to construct mjvScene // (by default we define XMJV to be the same as X) -#define MJMODEL_POINTERS \ - X ( mjtNum, qpos0, nq, 1 ) \ - X ( mjtNum, qpos_spring, nq, 1 ) \ - XMJV( int, body_parentid, nbody, 1 ) \ - XMJV( int, body_rootid, nbody, 1 ) \ - XMJV( int, body_weldid, nbody, 1 ) \ - XMJV( int, body_mocapid, nbody, 1 ) \ - XMJV( int, body_jntnum, nbody, 1 ) \ - XMJV( int, body_jntadr, nbody, 1 ) \ - XMJV( int, body_dofnum, nbody, 1 ) \ - XMJV( int, body_dofadr, nbody, 1 ) \ - X ( int, body_treeid, nbody, 1 ) \ - XMJV( int, body_geomnum, nbody, 1 ) \ - XMJV( int, body_geomadr, nbody, 1 ) \ - X ( mjtByte, body_simple, nbody, 1 ) \ - X ( mjtByte, body_sameframe, nbody, 1 ) \ - X ( mjtNum, body_pos, nbody, 3 ) \ - X ( mjtNum, body_quat, nbody, 4 ) \ - X ( mjtNum, body_ipos, nbody, 3 ) \ - XMJV( mjtNum, body_iquat, nbody, 4 ) \ - XMJV( mjtNum, body_mass, nbody, 1 ) \ - X ( mjtNum, body_subtreemass, nbody, 1 ) \ - XMJV( mjtNum, body_inertia, nbody, 3 ) \ - X ( mjtNum, body_invweight0, nbody, 2 ) \ - X ( mjtNum, body_gravcomp, nbody, 1 ) \ - X ( mjtNum, body_margin, nbody, 1 ) \ - X ( mjtNum, body_user, nbody, MJ_M(nuser_body) ) \ - X ( int, body_plugin, nbody, 1 ) \ - X ( int, body_contype, nbody, 1 ) \ - X ( int, body_conaffinity, nbody, 1 ) \ - XMJV( int, body_bvhadr, nbody, 1 ) \ - XMJV( int, body_bvhnum, nbody, 1 ) \ - XMJV( int, bvh_depth, nbvh, 1 ) \ - XMJV( int, bvh_child, nbvh, 2 ) \ - XMJV( int, bvh_nodeid, nbvh, 1 ) \ - XMJV( mjtNum, bvh_aabb, nbvhstatic, 6 ) \ - XMJV( int, jnt_type, njnt, 1 ) \ - X ( int, jnt_qposadr, njnt, 1 ) \ - X ( int, jnt_dofadr, njnt, 1 ) \ - XMJV( int, jnt_bodyid, njnt, 1 ) \ - XMJV( int, jnt_group, njnt, 1 ) \ - X ( mjtByte, jnt_limited, njnt, 1 ) \ - X ( mjtByte, jnt_actfrclimited, njnt, 1 ) \ - X ( mjtByte, jnt_actgravcomp, njnt, 1 ) \ - X ( mjtNum, jnt_solref, njnt, mjNREF ) \ - X ( mjtNum, jnt_solimp, njnt, mjNIMP ) \ - X ( mjtNum, jnt_pos, njnt, 3 ) \ - X ( mjtNum, jnt_axis, njnt, 3 ) \ - X ( mjtNum, jnt_stiffness, njnt, 1 ) \ - X ( mjtNum, jnt_range, njnt, 2 ) \ - X ( mjtNum, jnt_actfrcrange, njnt, 2 ) \ - X ( mjtNum, jnt_margin, njnt, 1 ) \ - X ( mjtNum, jnt_user, njnt, MJ_M(nuser_jnt) ) \ - X ( int, dof_bodyid, nv, 1 ) \ - X ( int, dof_jntid, nv, 1 ) \ - X ( int, dof_parentid, nv, 1 ) \ - X ( int, dof_treeid, nv, 1 ) \ - X ( int, dof_Madr, nv, 1 ) \ - X ( int, dof_simplenum, nv, 1 ) \ - X ( mjtNum, dof_solref, nv, mjNREF ) \ - X ( mjtNum, dof_solimp, nv, mjNIMP ) \ - X ( mjtNum, dof_frictionloss, nv, 1 ) \ - X ( mjtNum, dof_armature, nv, 1 ) \ - X ( mjtNum, dof_damping, nv, 1 ) \ - X ( mjtNum, dof_invweight0, nv, 1 ) \ - X ( mjtNum, dof_M0, nv, 1 ) \ - XMJV( int, geom_type, ngeom, 1 ) \ - XMJV( int, geom_contype, ngeom, 1 ) \ - XMJV( int, geom_conaffinity, ngeom, 1 ) \ - X ( int, geom_condim, ngeom, 1 ) \ - XMJV( int, geom_bodyid, ngeom, 1 ) \ - XMJV( int, geom_dataid, ngeom, 1 ) \ - XMJV( int, geom_matid, ngeom, 1 ) \ - XMJV( int, geom_group, ngeom, 1 ) \ - X ( int, geom_priority, ngeom, 1 ) \ - X ( int, geom_plugin, ngeom, 1 ) \ - X ( mjtByte, geom_sameframe, ngeom, 1 ) \ - X ( mjtNum, geom_solmix, ngeom, 1 ) \ - X ( mjtNum, geom_solref, ngeom, mjNREF ) \ - X ( mjtNum, geom_solimp, ngeom, mjNIMP ) \ - XMJV( mjtNum, geom_size, ngeom, 3 ) \ - XMJV( mjtNum, geom_aabb, ngeom, 6 ) \ - XMJV( mjtNum, geom_rbound, ngeom, 1 ) \ - X ( mjtNum, geom_pos, ngeom, 3 ) \ - X ( mjtNum, geom_quat, ngeom, 4 ) \ - X ( mjtNum, geom_friction, ngeom, 3 ) \ - X ( mjtNum, geom_margin, ngeom, 1 ) \ - X ( mjtNum, geom_gap, ngeom, 1 ) \ - X ( mjtNum, geom_fluid, ngeom, mjNFLUID ) \ - X ( mjtNum, geom_user, ngeom, MJ_M(nuser_geom) ) \ - XMJV( float, geom_rgba, ngeom, 4 ) \ - XMJV( int, site_type, nsite, 1 ) \ - XMJV( int, site_bodyid, nsite, 1 ) \ - XMJV( int, site_matid, nsite, 1 ) \ - XMJV( int, site_group, nsite, 1 ) \ - X ( mjtByte, site_sameframe, nsite, 1 ) \ - XMJV( mjtNum, site_size, nsite, 3 ) \ - X ( mjtNum, site_pos, nsite, 3 ) \ - X ( mjtNum, site_quat, nsite, 4 ) \ - X ( mjtNum, site_user, nsite, MJ_M(nuser_site) ) \ - XMJV( float, site_rgba, nsite, 4 ) \ - X ( int, cam_mode, ncam, 1 ) \ - X ( int, cam_bodyid, ncam, 1 ) \ - X ( int, cam_targetbodyid, ncam, 1 ) \ - X ( int, cam_resolution, ncam, 2 ) \ - XMJV( float, cam_sensorsize, ncam, 2 ) \ - XMJV( float, cam_intrinsic, ncam, 4 ) \ - X ( mjtNum, cam_pos, ncam, 3 ) \ - X ( mjtNum, cam_quat, ncam, 4 ) \ - X ( mjtNum, cam_poscom0, ncam, 3 ) \ - X ( mjtNum, cam_pos0, ncam, 3 ) \ - X ( mjtNum, cam_mat0, ncam, 9 ) \ - XMJV( mjtNum, cam_fovy, ncam, 1 ) \ - XMJV( mjtNum, cam_ipd, ncam, 1 ) \ - X ( mjtNum, cam_user, ncam, MJ_M(nuser_cam) ) \ - X ( int, light_mode, nlight, 1 ) \ - X ( int, light_bodyid, nlight, 1 ) \ - X ( int, light_targetbodyid, nlight, 1 ) \ - XMJV( mjtByte, light_directional, nlight, 1 ) \ - XMJV( mjtByte, light_castshadow, nlight, 1 ) \ - XMJV( mjtByte, light_active, nlight, 1 ) \ - X ( mjtNum, light_pos, nlight, 3 ) \ - X ( mjtNum, light_dir, nlight, 3 ) \ - X ( mjtNum, light_poscom0, nlight, 3 ) \ - X ( mjtNum, light_pos0, nlight, 3 ) \ - X ( mjtNum, light_dir0, nlight, 3 ) \ - XMJV( float, light_attenuation, nlight, 3 ) \ - XMJV( float, light_cutoff, nlight, 1 ) \ - XMJV( float, light_exponent, nlight, 1 ) \ - XMJV( float, light_ambient, nlight, 3 ) \ - XMJV( float, light_diffuse, nlight, 3 ) \ - XMJV( float, light_specular, nlight, 3 ) \ - X ( int, flex_contype, nflex, 1 ) \ - X ( int, flex_conaffinity, nflex, 1 ) \ - X ( int, flex_condim, nflex, 1 ) \ - X ( int, flex_priority, nflex, 1 ) \ - X ( mjtNum, flex_solmix, nflex, 1 ) \ - X ( mjtNum, flex_solref, nflex, mjNREF ) \ - X ( mjtNum, flex_solimp, nflex, mjNIMP ) \ - X ( mjtNum, flex_friction, nflex, 3 ) \ - X ( mjtNum, flex_margin, nflex, 1 ) \ - X ( mjtNum, flex_gap, nflex, 1 ) \ - X ( mjtByte, flex_internal, nflex, 1 ) \ - X ( int, flex_selfcollide, nflex, 1 ) \ - X ( int, flex_activelayers, nflex, 1 ) \ - XMJV( int, flex_dim, nflex, 1 ) \ - XMJV( int, flex_matid, nflex, 1 ) \ - XMJV( int, flex_group, nflex, 1 ) \ - XMJV( int, flex_vertadr, nflex, 1 ) \ - XMJV( int, flex_vertnum, nflex, 1 ) \ - X ( int, flex_edgeadr, nflex, 1 ) \ - X ( int, flex_edgenum, nflex, 1 ) \ - XMJV( int, flex_elemadr, nflex, 1 ) \ - XMJV( int, flex_elemnum, nflex, 1 ) \ - XMJV( int, flex_elemdataadr, nflex, 1 ) \ - XMJV( int, flex_shellnum, nflex, 1 ) \ - XMJV( int, flex_shelldataadr, nflex, 1 ) \ - X ( int, flex_evpairadr, nflex, 1 ) \ - X ( int, flex_evpairnum, nflex, 1 ) \ - XMJV( int, flex_texcoordadr, nflex, 1 ) \ - X ( int, flex_vertbodyid, nflexvert, 1 ) \ - X ( int, flex_edge, nflexedge, 2 ) \ - XMJV( int, flex_elem, nflexelemdata, 1 ) \ - XMJV( int, flex_elemlayer, nflexelem, 1 ) \ - XMJV( int, flex_shell, nflexshelldata,1 ) \ - X ( int, flex_evpair, nflexevpair, 2 ) \ - X ( mjtNum, flex_vert, nflexvert, 3 ) \ - X ( mjtNum, flex_xvert0, nflexvert, 3 ) \ - X ( mjtNum, flexedge_length0, nflexedge, 1 ) \ - X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \ - XMJV( mjtNum, flex_radius, nflex, 1 ) \ - X ( mjtNum, flex_edgestiffness, nflex, 1 ) \ - X ( mjtNum, flex_edgedamping, nflex, 1 ) \ - X ( mjtByte, flex_edgeequality, nflex, 1 ) \ - X ( mjtByte, flex_rigid, nflex, 1 ) \ - X ( mjtByte, flexedge_rigid, nflexedge, 1 ) \ - X ( mjtByte, flex_centered, nflex, 1 ) \ - XMJV( mjtByte, flex_flatskin, nflex, 1 ) \ - XMJV( int, flex_bvhadr, nflex, 1 ) \ - XMJV( int, flex_bvhnum, nflex, 1 ) \ - XMJV( float, flex_rgba, nflex, 4 ) \ - X ( float, flex_texcoord, nflextexcoord, 2 ) \ - X ( int, mesh_vertadr, nmesh, 1 ) \ - X ( int, mesh_vertnum, nmesh, 1 ) \ - X ( int, mesh_normaladr, nmesh, 1 ) \ - X ( int, mesh_normalnum, nmesh, 1 ) \ - XMJV( int, mesh_texcoordadr, nmesh, 1 ) \ - X ( int, mesh_texcoordnum, nmesh, 1 ) \ - X ( int, mesh_faceadr, nmesh, 1 ) \ - X ( int, mesh_facenum, nmesh, 1 ) \ - XMJV( int, mesh_bvhadr, nmesh, 1 ) \ - XMJV( int, mesh_bvhnum, nmesh, 1 ) \ - XMJV( int, mesh_graphadr, nmesh, 1 ) \ - X ( mjtNum, mesh_pos, nmesh, 3 ) \ - X ( mjtNum, mesh_quat, nmesh, 4 ) \ - X ( float, mesh_vert, nmeshvert, 3 ) \ - X ( float, mesh_normal, nmeshnormal, 3 ) \ - X ( float, mesh_texcoord, nmeshtexcoord, 2 ) \ - X ( int, mesh_face, nmeshface, 3 ) \ - X ( int, mesh_facenormal, nmeshface, 3 ) \ - X ( int, mesh_facetexcoord, nmeshface, 3 ) \ - X ( int, mesh_graph, nmeshgraph, 1 ) \ - XMJV( int, mesh_pathadr, nmesh, 1 ) \ - XMJV( int, skin_matid, nskin, 1 ) \ - XMJV( int, skin_group, nskin, 1 ) \ - XMJV( float, skin_rgba, nskin, 4 ) \ - XMJV( float, skin_inflate, nskin, 1 ) \ - XMJV( int, skin_vertadr, nskin, 1 ) \ - XMJV( int, skin_vertnum, nskin, 1 ) \ - XMJV( int, skin_texcoordadr, nskin, 1 ) \ - XMJV( int, skin_faceadr, nskin, 1 ) \ - XMJV( int, skin_facenum, nskin, 1 ) \ - XMJV( int, skin_boneadr, nskin, 1 ) \ - XMJV( int, skin_bonenum, nskin, 1 ) \ - XMJV( float, skin_vert, nskinvert, 3 ) \ - X ( float, skin_texcoord, nskintexvert, 2 ) \ - XMJV( int, skin_face, nskinface, 3 ) \ - XMJV( int, skin_bonevertadr, nskinbone, 1 ) \ - XMJV( int, skin_bonevertnum, nskinbone, 1 ) \ - XMJV( float, skin_bonebindpos, nskinbone, 3 ) \ - XMJV( float, skin_bonebindquat, nskinbone, 4 ) \ - XMJV( int, skin_bonebodyid, nskinbone, 1 ) \ - XMJV( int, skin_bonevertid, nskinbonevert, 1 ) \ - XMJV( float, skin_bonevertweight, nskinbonevert, 1 ) \ - XMJV( int, skin_pathadr, nskin, 1 ) \ - X ( mjtNum, hfield_size, nhfield, 4 ) \ - X ( int, hfield_nrow, nhfield, 1 ) \ - X ( int, hfield_ncol, nhfield, 1 ) \ - X ( int, hfield_adr, nhfield, 1 ) \ - X ( float, hfield_data, nhfielddata, 1 ) \ - XMJV( int, hfield_pathadr, nhfield, 1 ) \ - X ( int, tex_type, ntex, 1 ) \ - X ( int, tex_height, ntex, 1 ) \ - X ( int, tex_width, ntex, 1 ) \ - X ( int, tex_adr, ntex, 1 ) \ - X ( mjtByte, tex_rgb, ntexdata, 1 ) \ - XMJV( int, tex_pathadr, ntex, 1 ) \ - XMJV( int, mat_texid, nmat, 1 ) \ - XMJV( mjtByte, mat_texuniform, nmat, 1 ) \ - XMJV( float, mat_texrepeat, nmat, 2 ) \ - XMJV( float, mat_emission, nmat, 1 ) \ - XMJV( float, mat_specular, nmat, 1 ) \ - XMJV( float, mat_shininess, nmat, 1 ) \ - XMJV( float, mat_reflectance, nmat, 1 ) \ - XMJV( float, mat_rgba, nmat, 4 ) \ - X ( int, pair_dim, npair, 1 ) \ - X ( int, pair_geom1, npair, 1 ) \ - X ( int, pair_geom2, npair, 1 ) \ - X ( int, pair_signature, npair, 1 ) \ - X ( mjtNum, pair_solref, npair, mjNREF ) \ - X ( mjtNum, pair_solreffriction, npair, mjNREF ) \ - X ( mjtNum, pair_solimp, npair, mjNIMP ) \ - X ( mjtNum, pair_margin, npair, 1 ) \ - X ( mjtNum, pair_gap, npair, 1 ) \ - X ( mjtNum, pair_friction, npair, 5 ) \ - X ( int, exclude_signature, nexclude, 1 ) \ - XMJV( int, eq_type, neq, 1 ) \ - XMJV( int, eq_obj1id, neq, 1 ) \ - XMJV( int, eq_obj2id, neq, 1 ) \ - X ( mjtByte, eq_active0, neq, 1 ) \ - X ( mjtNum, eq_solref, neq, mjNREF ) \ - X ( mjtNum, eq_solimp, neq, mjNIMP ) \ - XMJV( mjtNum, eq_data, neq, mjNEQDATA ) \ - X ( int, tendon_adr, ntendon, 1 ) \ - XMJV( int, tendon_num, ntendon, 1 ) \ - XMJV( int, tendon_matid, ntendon, 1 ) \ - XMJV( int, tendon_group, ntendon, 1 ) \ - XMJV( mjtByte, tendon_limited, ntendon, 1 ) \ - XMJV( mjtNum, tendon_width, ntendon, 1 ) \ - X ( mjtNum, tendon_solref_lim, ntendon, mjNREF ) \ - X ( mjtNum, tendon_solimp_lim, ntendon, mjNIMP ) \ - X ( mjtNum, tendon_solref_fri, ntendon, mjNREF ) \ - X ( mjtNum, tendon_solimp_fri, ntendon, mjNIMP ) \ - XMJV( mjtNum, tendon_range, ntendon, 2 ) \ - X ( mjtNum, tendon_margin, ntendon, 1 ) \ - XMJV( mjtNum, tendon_stiffness, ntendon, 1 ) \ - XMJV( mjtNum, tendon_damping, ntendon, 1 ) \ - XMJV( mjtNum, tendon_frictionloss, ntendon, 1 ) \ - XMJV( mjtNum, tendon_lengthspring, ntendon, 2 ) \ - X ( mjtNum, tendon_length0, ntendon, 1 ) \ - X ( mjtNum, tendon_invweight0, ntendon, 1 ) \ - X ( mjtNum, tendon_user, ntendon, MJ_M(nuser_tendon) ) \ - XMJV( float, tendon_rgba, ntendon, 4 ) \ - X ( int, wrap_type, nwrap, 1 ) \ - X ( int, wrap_objid, nwrap, 1 ) \ - X ( mjtNum, wrap_prm, nwrap, 1 ) \ - XMJV( int, actuator_trntype, nu, 1 ) \ - XMJV( int, actuator_dyntype, nu, 1 ) \ - X ( int, actuator_gaintype, nu, 1 ) \ - X ( int, actuator_biastype, nu, 1 ) \ - XMJV( int, actuator_trnid, nu, 2 ) \ - XMJV( int, actuator_actadr, nu, 1 ) \ - XMJV( int, actuator_actnum, nu, 1 ) \ - XMJV( int, actuator_group, nu, 1 ) \ - XMJV( mjtByte, actuator_ctrllimited, nu, 1 ) \ - X ( mjtByte, actuator_forcelimited, nu, 1 ) \ - XMJV( mjtByte, actuator_actlimited, nu, 1 ) \ - X ( mjtNum, actuator_dynprm, nu, mjNDYN ) \ - X ( mjtNum, actuator_gainprm, nu, mjNGAIN ) \ - X ( mjtNum, actuator_biasprm, nu, mjNBIAS ) \ - X ( mjtByte, actuator_actearly, nu, 1 ) \ - XMJV( mjtNum, actuator_ctrlrange, nu, 2 ) \ - X ( mjtNum, actuator_forcerange, nu, 2 ) \ - XMJV( mjtNum, actuator_actrange, nu, 2 ) \ - X ( mjtNum, actuator_gear, nu, 6 ) \ - XMJV( mjtNum, actuator_cranklength, nu, 1 ) \ - X ( mjtNum, actuator_acc0, nu, 1 ) \ - X ( mjtNum, actuator_length0, nu, 1 ) \ - X ( mjtNum, actuator_lengthrange, nu, 2 ) \ - X ( mjtNum, actuator_user, nu, MJ_M(nuser_actuator) ) \ - X ( int, actuator_plugin, nu, 1 ) \ - XMJV( int, sensor_type, nsensor, 1 ) \ - X ( int, sensor_datatype, nsensor, 1 ) \ - X ( int, sensor_needstage, nsensor, 1 ) \ - X ( int, sensor_objtype, nsensor, 1 ) \ - XMJV( int, sensor_objid, nsensor, 1 ) \ - X ( int, sensor_reftype, nsensor, 1 ) \ - X ( int, sensor_refid, nsensor, 1 ) \ - X ( int, sensor_dim, nsensor, 1 ) \ - XMJV( int, sensor_adr, nsensor, 1 ) \ - X ( mjtNum, sensor_cutoff, nsensor, 1 ) \ - X ( mjtNum, sensor_noise, nsensor, 1 ) \ - X ( mjtNum, sensor_user, nsensor, MJ_M(nuser_sensor) ) \ - X ( int, sensor_plugin, nsensor, 1 ) \ - X ( int, plugin, nplugin, 1 ) \ - X ( int, plugin_stateadr, nplugin, 1 ) \ - X ( int, plugin_statenum, nplugin, 1 ) \ - X ( char, plugin_attr, npluginattr, 1 ) \ - X ( int, plugin_attradr, nplugin, 1 ) \ - X ( int, numeric_adr, nnumeric, 1 ) \ - X ( int, numeric_size, nnumeric, 1 ) \ - X ( mjtNum, numeric_data, nnumericdata, 1 ) \ - X ( int, text_adr, ntext, 1 ) \ - X ( int, text_size, ntext, 1 ) \ - X ( char, text_data, ntextdata, 1 ) \ - X ( int, tuple_adr, ntuple, 1 ) \ - X ( int, tuple_size, ntuple, 1 ) \ - X ( int, tuple_objtype, ntupledata, 1 ) \ - X ( int, tuple_objid, ntupledata, 1 ) \ - X ( mjtNum, tuple_objprm, ntupledata, 1 ) \ - X ( mjtNum, key_time, nkey, 1 ) \ - X ( mjtNum, key_qpos, nkey, MJ_M(nq) ) \ - X ( mjtNum, key_qvel, nkey, MJ_M(nv) ) \ - X ( mjtNum, key_act, nkey, MJ_M(na) ) \ - X ( mjtNum, key_mpos, nkey, MJ_M(nmocap)*3 ) \ - X ( mjtNum, key_mquat, nkey, MJ_M(nmocap)*4 ) \ - X ( mjtNum, key_ctrl, nkey, MJ_M(nu) ) \ - XMJV( int, name_bodyadr, nbody, 1 ) \ - XMJV( int, name_jntadr, njnt, 1 ) \ - XMJV( int, name_geomadr, ngeom, 1 ) \ - XMJV( int, name_siteadr, nsite, 1 ) \ - XMJV( int, name_camadr, ncam, 1 ) \ - XMJV( int, name_lightadr, nlight, 1 ) \ - X ( int, name_flexadr, nflex, 1 ) \ - X ( int, name_meshadr, nmesh, 1 ) \ - X ( int, name_skinadr, nskin, 1 ) \ - X ( int, name_hfieldadr, nhfield, 1 ) \ - X ( int, name_texadr, ntex, 1 ) \ - X ( int, name_matadr, nmat, 1 ) \ - X ( int, name_pairadr, npair, 1 ) \ - X ( int, name_excludeadr, nexclude, 1 ) \ - XMJV( int, name_eqadr, neq, 1 ) \ - XMJV( int, name_tendonadr, ntendon, 1 ) \ - XMJV( int, name_actuatoradr, nu, 1 ) \ - X ( int, name_sensoradr, nsensor, 1 ) \ - X ( int, name_numericadr, nnumeric, 1 ) \ - X ( int, name_textadr, ntext, 1 ) \ - X ( int, name_tupleadr, ntuple, 1 ) \ - X ( int, name_keyadr, nkey, 1 ) \ - X ( int, name_pluginadr, nplugin, 1 ) \ - XMJV( char, names, nnames, 1 ) \ - X ( int, names_map, nnames_map, 1 ) \ - XMJV( char, paths, npaths, 1 ) \ - -//-------------------------------- mjData ---------------------------------------------------------- +#define MJMODEL_POINTERS \ + X(mjtNum, qpos0, nq, 1) \ + X(mjtNum, qpos_spring, nq, 1) \ + XMJV(int, body_parentid, nbody, 1) \ + XMJV(int, body_rootid, nbody, 1) \ + XMJV(int, body_weldid, nbody, 1) \ + XMJV(int, body_mocapid, nbody, 1) \ + XMJV(int, body_jntnum, nbody, 1) \ + XMJV(int, body_jntadr, nbody, 1) \ + XMJV(int, body_dofnum, nbody, 1) \ + XMJV(int, body_dofadr, nbody, 1) \ + X(int, body_treeid, nbody, 1) \ + XMJV(int, body_geomnum, nbody, 1) \ + XMJV(int, body_geomadr, nbody, 1) \ + X(mjtByte, body_simple, nbody, 1) \ + X(mjtByte, body_sameframe, nbody, 1) \ + X(mjtNum, body_pos, nbody, 3) \ + X(mjtNum, body_quat, nbody, 4) \ + X(mjtNum, body_ipos, nbody, 3) \ + XMJV(mjtNum, body_iquat, nbody, 4) \ + XMJV(mjtNum, body_mass, nbody, 1) \ + X(mjtNum, body_subtreemass, nbody, 1) \ + XMJV(mjtNum, body_inertia, nbody, 3) \ + X(mjtNum, body_invweight0, nbody, 2) \ + X(mjtNum, body_gravcomp, nbody, 1) \ + X(mjtNum, body_margin, nbody, 1) \ + X(mjtNum, body_user, nbody, MJ_M(nuser_body)) \ + X(int, body_plugin, nbody, 1) \ + X(int, body_contype, nbody, 1) \ + X(int, body_conaffinity, nbody, 1) \ + XMJV(int, body_bvhadr, nbody, 1) \ + XMJV(int, body_bvhnum, nbody, 1) \ + XMJV(int, bvh_depth, nbvh, 1) \ + XMJV(int, bvh_child, nbvh, 2) \ + XMJV(int, bvh_nodeid, nbvh, 1) \ + XMJV(mjtNum, bvh_aabb, nbvhstatic, 6) \ + XMJV(int, jnt_type, njnt, 1) \ + X(int, jnt_qposadr, njnt, 1) \ + X(int, jnt_dofadr, njnt, 1) \ + XMJV(int, jnt_bodyid, njnt, 1) \ + XMJV(int, jnt_group, njnt, 1) \ + X(mjtByte, jnt_limited, njnt, 1) \ + X(mjtByte, jnt_actfrclimited, njnt, 1) \ + X(mjtByte, jnt_actgravcomp, njnt, 1) \ + X(mjtNum, jnt_solref, njnt, mjNREF) \ + X(mjtNum, jnt_solimp, njnt, mjNIMP) \ + X(mjtNum, jnt_pos, njnt, 3) \ + X(mjtNum, jnt_axis, njnt, 3) \ + X(mjtNum, jnt_stiffness, njnt, 1) \ + X(mjtNum, jnt_range, njnt, 2) \ + X(mjtNum, jnt_actfrcrange, njnt, 2) \ + X(mjtNum, jnt_margin, njnt, 1) \ + X(mjtNum, jnt_user, njnt, MJ_M(nuser_jnt)) \ + X(int, dof_bodyid, nv, 1) \ + X(int, dof_jntid, nv, 1) \ + X(int, dof_parentid, nv, 1) \ + X(int, dof_treeid, nv, 1) \ + X(int, dof_Madr, nv, 1) \ + X(int, dof_simplenum, nv, 1) \ + X(mjtNum, dof_solref, nv, mjNREF) \ + X(mjtNum, dof_solimp, nv, mjNIMP) \ + X(mjtNum, dof_frictionloss, nv, 1) \ + X(mjtNum, dof_armature, nv, 1) \ + X(mjtNum, dof_damping, nv, 1) \ + X(mjtNum, dof_invweight0, nv, 1) \ + X(mjtNum, dof_M0, nv, 1) \ + XMJV(int, geom_type, ngeom, 1) \ + XMJV(int, geom_contype, ngeom, 1) \ + XMJV(int, geom_conaffinity, ngeom, 1) \ + X(int, geom_condim, ngeom, 1) \ + XMJV(int, geom_bodyid, ngeom, 1) \ + XMJV(int, geom_dataid, ngeom, 1) \ + XMJV(int, geom_matid, ngeom, 1) \ + XMJV(int, geom_group, ngeom, 1) \ + X(int, geom_priority, ngeom, 1) \ + X(int, geom_plugin, ngeom, 1) \ + X(mjtByte, geom_sameframe, ngeom, 1) \ + X(mjtNum, geom_solmix, ngeom, 1) \ + X(mjtNum, geom_solref, ngeom, mjNREF) \ + X(mjtNum, geom_solimp, ngeom, mjNIMP) \ + XMJV(mjtNum, geom_size, ngeom, 3) \ + XMJV(mjtNum, geom_aabb, ngeom, 6) \ + XMJV(mjtNum, geom_rbound, ngeom, 1) \ + X(mjtNum, geom_pos, ngeom, 3) \ + X(mjtNum, geom_quat, ngeom, 4) \ + X(mjtNum, geom_friction, ngeom, 3) \ + X(mjtNum, geom_margin, ngeom, 1) \ + X(mjtNum, geom_gap, ngeom, 1) \ + X(mjtNum, geom_fluid, ngeom, mjNFLUID) \ + X(mjtNum, geom_user, ngeom, MJ_M(nuser_geom)) \ + XMJV(float, geom_rgba, ngeom, 4) \ + XMJV(int, site_type, nsite, 1) \ + XMJV(int, site_bodyid, nsite, 1) \ + XMJV(int, site_matid, nsite, 1) \ + XMJV(int, site_group, nsite, 1) \ + X(mjtByte, site_sameframe, nsite, 1) \ + XMJV(mjtNum, site_size, nsite, 3) \ + X(mjtNum, site_pos, nsite, 3) \ + X(mjtNum, site_quat, nsite, 4) \ + X(mjtNum, site_user, nsite, MJ_M(nuser_site)) \ + XMJV(float, site_rgba, nsite, 4) \ + X(int, cam_mode, ncam, 1) \ + X(int, cam_bodyid, ncam, 1) \ + X(int, cam_targetbodyid, ncam, 1) \ + X(int, cam_resolution, ncam, 2) \ + XMJV(float, cam_sensorsize, ncam, 2) \ + XMJV(float, cam_intrinsic, ncam, 4) \ + X(mjtNum, cam_pos, ncam, 3) \ + X(mjtNum, cam_quat, ncam, 4) \ + X(mjtNum, cam_poscom0, ncam, 3) \ + X(mjtNum, cam_pos0, ncam, 3) \ + X(mjtNum, cam_mat0, ncam, 9) \ + XMJV(mjtNum, cam_fovy, ncam, 1) \ + XMJV(mjtNum, cam_ipd, ncam, 1) \ + X(mjtNum, cam_user, ncam, MJ_M(nuser_cam)) \ + X(int, light_mode, nlight, 1) \ + X(int, light_bodyid, nlight, 1) \ + X(int, light_targetbodyid, nlight, 1) \ + XMJV(mjtByte, light_directional, nlight, 1) \ + XMJV(mjtByte, light_castshadow, nlight, 1) \ + XMJV(mjtByte, light_active, nlight, 1) \ + X(mjtNum, light_pos, nlight, 3) \ + X(mjtNum, light_dir, nlight, 3) \ + X(mjtNum, light_poscom0, nlight, 3) \ + X(mjtNum, light_pos0, nlight, 3) \ + X(mjtNum, light_dir0, nlight, 3) \ + XMJV(float, light_attenuation, nlight, 3) \ + XMJV(float, light_cutoff, nlight, 1) \ + XMJV(float, light_exponent, nlight, 1) \ + XMJV(float, light_ambient, nlight, 3) \ + XMJV(float, light_diffuse, nlight, 3) \ + XMJV(float, light_specular, nlight, 3) \ + X(int, flex_contype, nflex, 1) \ + X(int, flex_conaffinity, nflex, 1) \ + X(int, flex_condim, nflex, 1) \ + X(int, flex_priority, nflex, 1) \ + X(mjtNum, flex_solmix, nflex, 1) \ + X(mjtNum, flex_solref, nflex, mjNREF) \ + X(mjtNum, flex_solimp, nflex, mjNIMP) \ + X(mjtNum, flex_friction, nflex, 3) \ + X(mjtNum, flex_margin, nflex, 1) \ + X(mjtNum, flex_gap, nflex, 1) \ + X(mjtByte, flex_internal, nflex, 1) \ + X(int, flex_selfcollide, nflex, 1) \ + X(int, flex_activelayers, nflex, 1) \ + XMJV(int, flex_dim, nflex, 1) \ + XMJV(int, flex_matid, nflex, 1) \ + XMJV(int, flex_group, nflex, 1) \ + XMJV(int, flex_vertadr, nflex, 1) \ + XMJV(int, flex_vertnum, nflex, 1) \ + X(int, flex_edgeadr, nflex, 1) \ + X(int, flex_edgenum, nflex, 1) \ + XMJV(int, flex_elemadr, nflex, 1) \ + XMJV(int, flex_elemnum, nflex, 1) \ + XMJV(int, flex_elemdataadr, nflex, 1) \ + XMJV(int, flex_shellnum, nflex, 1) \ + XMJV(int, flex_shelldataadr, nflex, 1) \ + X(int, flex_evpairadr, nflex, 1) \ + X(int, flex_evpairnum, nflex, 1) \ + XMJV(int, flex_texcoordadr, nflex, 1) \ + X(int, flex_vertbodyid, nflexvert, 1) \ + X(int, flex_edge, nflexedge, 2) \ + XMJV(int, flex_elem, nflexelemdata, 1) \ + XMJV(int, flex_elemlayer, nflexelem, 1) \ + XMJV(int, flex_shell, nflexshelldata, 1) \ + X(int, flex_evpair, nflexevpair, 2) \ + X(mjtNum, flex_vert, nflexvert, 3) \ + X(mjtNum, flex_xvert0, nflexvert, 3) \ + X(mjtNum, flexedge_length0, nflexedge, 1) \ + X(mjtNum, flexedge_invweight0, nflexedge, 1) \ + XMJV(mjtNum, flex_radius, nflex, 1) \ + X(mjtNum, flex_edgestiffness, nflex, 1) \ + X(mjtNum, flex_edgedamping, nflex, 1) \ + X(mjtByte, flex_edgeequality, nflex, 1) \ + X(mjtByte, flex_rigid, nflex, 1) \ + X(mjtByte, flexedge_rigid, nflexedge, 1) \ + X(mjtByte, flex_centered, nflex, 1) \ + XMJV(mjtByte, flex_flatskin, nflex, 1) \ + XMJV(int, flex_bvhadr, nflex, 1) \ + XMJV(int, flex_bvhnum, nflex, 1) \ + XMJV(float, flex_rgba, nflex, 4) \ + X(float, flex_texcoord, nflextexcoord, 2) \ + X(int, mesh_vertadr, nmesh, 1) \ + X(int, mesh_vertnum, nmesh, 1) \ + X(int, mesh_normaladr, nmesh, 1) \ + X(int, mesh_normalnum, nmesh, 1) \ + XMJV(int, mesh_texcoordadr, nmesh, 1) \ + X(int, mesh_texcoordnum, nmesh, 1) \ + X(int, mesh_faceadr, nmesh, 1) \ + X(int, mesh_facenum, nmesh, 1) \ + XMJV(int, mesh_bvhadr, nmesh, 1) \ + XMJV(int, mesh_bvhnum, nmesh, 1) \ + XMJV(int, mesh_graphadr, nmesh, 1) \ + X(mjtNum, mesh_pos, nmesh, 3) \ + X(mjtNum, mesh_quat, nmesh, 4) \ + X(float, mesh_vert, nmeshvert, 3) \ + X(float, mesh_normal, nmeshnormal, 3) \ + X(float, mesh_texcoord, nmeshtexcoord, 2) \ + X(int, mesh_face, nmeshface, 3) \ + X(int, mesh_facenormal, nmeshface, 3) \ + X(int, mesh_facetexcoord, nmeshface, 3) \ + X(int, mesh_graph, nmeshgraph, 1) \ + XMJV(int, mesh_pathadr, nmesh, 1) \ + XMJV(int, skin_matid, nskin, 1) \ + XMJV(int, skin_group, nskin, 1) \ + XMJV(float, skin_rgba, nskin, 4) \ + XMJV(float, skin_inflate, nskin, 1) \ + XMJV(int, skin_vertadr, nskin, 1) \ + XMJV(int, skin_vertnum, nskin, 1) \ + XMJV(int, skin_texcoordadr, nskin, 1) \ + XMJV(int, skin_faceadr, nskin, 1) \ + XMJV(int, skin_facenum, nskin, 1) \ + XMJV(int, skin_boneadr, nskin, 1) \ + XMJV(int, skin_bonenum, nskin, 1) \ + XMJV(float, skin_vert, nskinvert, 3) \ + X(float, skin_texcoord, nskintexvert, 2) \ + XMJV(int, skin_face, nskinface, 3) \ + XMJV(int, skin_bonevertadr, nskinbone, 1) \ + XMJV(int, skin_bonevertnum, nskinbone, 1) \ + XMJV(float, skin_bonebindpos, nskinbone, 3) \ + XMJV(float, skin_bonebindquat, nskinbone, 4) \ + XMJV(int, skin_bonebodyid, nskinbone, 1) \ + XMJV(int, skin_bonevertid, nskinbonevert, 1) \ + XMJV(float, skin_bonevertweight, nskinbonevert, 1) \ + XMJV(int, skin_pathadr, nskin, 1) \ + X(mjtNum, hfield_size, nhfield, 4) \ + X(int, hfield_nrow, nhfield, 1) \ + X(int, hfield_ncol, nhfield, 1) \ + X(int, hfield_adr, nhfield, 1) \ + X(float, hfield_data, nhfielddata, 1) \ + XMJV(int, hfield_pathadr, nhfield, 1) \ + X(int, tex_type, ntex, 1) \ + X(int, tex_height, ntex, 1) \ + X(int, tex_width, ntex, 1) \ + X(int, tex_adr, ntex, 1) \ + X(mjtByte, tex_rgb, ntexdata, 1) \ + XMJV(int, tex_pathadr, ntex, 1) \ + XMJV(int, mat_texid, nmat, 1) \ + XMJV(mjtByte, mat_texuniform, nmat, 1) \ + XMJV(float, mat_texrepeat, nmat, 2) \ + XMJV(float, mat_emission, nmat, 1) \ + XMJV(float, mat_specular, nmat, 1) \ + XMJV(float, mat_shininess, nmat, 1) \ + XMJV(float, mat_reflectance, nmat, 1) \ + XMJV(float, mat_rgba, nmat, 4) \ + X(int, pair_dim, npair, 1) \ + X(int, pair_geom1, npair, 1) \ + X(int, pair_geom2, npair, 1) \ + X(int, pair_signature, npair, 1) \ + X(mjtNum, pair_solref, npair, mjNREF) \ + X(mjtNum, pair_solreffriction, npair, mjNREF) \ + X(mjtNum, pair_solimp, npair, mjNIMP) \ + X(mjtNum, pair_margin, npair, 1) \ + X(mjtNum, pair_gap, npair, 1) \ + X(mjtNum, pair_friction, npair, 5) \ + X(int, exclude_signature, nexclude, 1) \ + XMJV(int, eq_type, neq, 1) \ + XMJV(int, eq_obj1id, neq, 1) \ + XMJV(int, eq_obj2id, neq, 1) \ + X(mjtByte, eq_active0, neq, 1) \ + X(mjtNum, eq_solref, neq, mjNREF) \ + X(mjtNum, eq_solimp, neq, mjNIMP) \ + XMJV(mjtNum, eq_data, neq, mjNEQDATA) \ + X(int, tendon_adr, ntendon, 1) \ + XMJV(int, tendon_num, ntendon, 1) \ + XMJV(int, tendon_matid, ntendon, 1) \ + XMJV(int, tendon_group, ntendon, 1) \ + XMJV(mjtByte, tendon_limited, ntendon, 1) \ + XMJV(mjtNum, tendon_width, ntendon, 1) \ + X(mjtNum, tendon_solref_lim, ntendon, mjNREF) \ + X(mjtNum, tendon_solimp_lim, ntendon, mjNIMP) \ + X(mjtNum, tendon_solref_fri, ntendon, mjNREF) \ + X(mjtNum, tendon_solimp_fri, ntendon, mjNIMP) \ + XMJV(mjtNum, tendon_range, ntendon, 2) \ + X(mjtNum, tendon_margin, ntendon, 1) \ + XMJV(mjtNum, tendon_stiffness, ntendon, 1) \ + XMJV(mjtNum, tendon_damping, ntendon, 1) \ + XMJV(mjtNum, tendon_frictionloss, ntendon, 1) \ + XMJV(mjtNum, tendon_lengthspring, ntendon, 2) \ + X(mjtNum, tendon_length0, ntendon, 1) \ + X(mjtNum, tendon_invweight0, ntendon, 1) \ + X(mjtNum, tendon_user, ntendon, MJ_M(nuser_tendon)) \ + XMJV(float, tendon_rgba, ntendon, 4) \ + X(int, wrap_type, nwrap, 1) \ + X(int, wrap_objid, nwrap, 1) \ + X(mjtNum, wrap_prm, nwrap, 1) \ + XMJV(int, actuator_trntype, nu, 1) \ + XMJV(int, actuator_dyntype, nu, 1) \ + X(int, actuator_gaintype, nu, 1) \ + X(int, actuator_biastype, nu, 1) \ + XMJV(int, actuator_trnid, nu, 2) \ + XMJV(int, actuator_actadr, nu, 1) \ + XMJV(int, actuator_actnum, nu, 1) \ + XMJV(int, actuator_group, nu, 1) \ + XMJV(mjtByte, actuator_ctrllimited, nu, 1) \ + X(mjtByte, actuator_forcelimited, nu, 1) \ + XMJV(mjtByte, actuator_actlimited, nu, 1) \ + X(mjtNum, actuator_dynprm, nu, mjNDYN) \ + X(mjtNum, actuator_gainprm, nu, mjNGAIN) \ + X(mjtNum, actuator_biasprm, nu, mjNBIAS) \ + X(mjtByte, actuator_actearly, nu, 1) \ + XMJV(mjtNum, actuator_ctrlrange, nu, 2) \ + X(mjtNum, actuator_forcerange, nu, 2) \ + XMJV(mjtNum, actuator_actrange, nu, 2) \ + X(mjtNum, actuator_gear, nu, 6) \ + XMJV(mjtNum, actuator_cranklength, nu, 1) \ + X(mjtNum, actuator_acc0, nu, 1) \ + X(mjtNum, actuator_length0, nu, 1) \ + X(mjtNum, actuator_lengthrange, nu, 2) \ + X(mjtNum, actuator_user, nu, MJ_M(nuser_actuator)) \ + X(int, actuator_plugin, nu, 1) \ + XMJV(int, sensor_type, nsensor, 1) \ + X(int, sensor_datatype, nsensor, 1) \ + X(int, sensor_needstage, nsensor, 1) \ + X(int, sensor_objtype, nsensor, 1) \ + XMJV(int, sensor_objid, nsensor, 1) \ + X(int, sensor_reftype, nsensor, 1) \ + X(int, sensor_refid, nsensor, 1) \ + X(int, sensor_dim, nsensor, 1) \ + XMJV(int, sensor_adr, nsensor, 1) \ + X(mjtNum, sensor_cutoff, nsensor, 1) \ + X(mjtNum, sensor_noise, nsensor, 1) \ + X(mjtNum, sensor_user, nsensor, MJ_M(nuser_sensor)) \ + X(int, sensor_plugin, nsensor, 1) \ + X(int, plugin, nplugin, 1) \ + X(int, plugin_stateadr, nplugin, 1) \ + X(int, plugin_statenum, nplugin, 1) \ + X(char, plugin_attr, npluginattr, 1) \ + X(int, plugin_attradr, nplugin, 1) \ + X(int, numeric_adr, nnumeric, 1) \ + X(int, numeric_size, nnumeric, 1) \ + X(mjtNum, numeric_data, nnumericdata, 1) \ + X(int, text_adr, ntext, 1) \ + X(int, text_size, ntext, 1) \ + X(char, text_data, ntextdata, 1) \ + X(int, tuple_adr, ntuple, 1) \ + X(int, tuple_size, ntuple, 1) \ + X(int, tuple_objtype, ntupledata, 1) \ + X(int, tuple_objid, ntupledata, 1) \ + X(mjtNum, tuple_objprm, ntupledata, 1) \ + X(mjtNum, key_time, nkey, 1) \ + X(mjtNum, key_qpos, nkey, MJ_M(nq)) \ + X(mjtNum, key_qvel, nkey, MJ_M(nv)) \ + X(mjtNum, key_act, nkey, MJ_M(na)) \ + X(mjtNum, key_mpos, nkey, MJ_M(nmocap) * 3) \ + X(mjtNum, key_mquat, nkey, MJ_M(nmocap) * 4) \ + X(mjtNum, key_ctrl, nkey, MJ_M(nu)) \ + XMJV(int, name_bodyadr, nbody, 1) \ + XMJV(int, name_jntadr, njnt, 1) \ + XMJV(int, name_geomadr, ngeom, 1) \ + XMJV(int, name_siteadr, nsite, 1) \ + XMJV(int, name_camadr, ncam, 1) \ + XMJV(int, name_lightadr, nlight, 1) \ + X(int, name_flexadr, nflex, 1) \ + X(int, name_meshadr, nmesh, 1) \ + X(int, name_skinadr, nskin, 1) \ + X(int, name_hfieldadr, nhfield, 1) \ + X(int, name_texadr, ntex, 1) \ + X(int, name_matadr, nmat, 1) \ + X(int, name_pairadr, npair, 1) \ + X(int, name_excludeadr, nexclude, 1) \ + XMJV(int, name_eqadr, neq, 1) \ + XMJV(int, name_tendonadr, ntendon, 1) \ + XMJV(int, name_actuatoradr, nu, 1) \ + X(int, name_sensoradr, nsensor, 1) \ + X(int, name_numericadr, nnumeric, 1) \ + X(int, name_textadr, ntext, 1) \ + X(int, name_tupleadr, ntuple, 1) \ + X(int, name_keyadr, nkey, 1) \ + X(int, name_pluginadr, nplugin, 1) \ + XMJV(char, names, nnames, 1) \ + X(int, names_map, nnames_map, 1) \ + XMJV(char, paths, npaths, 1) + +//-------------------------------- mjData +//---------------------------------------------------------- // define symbols needed in MJDATA_POINTERS (corresponding to number of columns) -#define MJDATA_POINTERS_PREAMBLE( m ) \ - int nv = m->nv; - +#define MJDATA_POINTERS_PREAMBLE(m) int nv = m->nv; // pointer fields of mjData // XMJV means that the field is required to construct mjvScene // (by default we define XMJV to be the same as X) -#define MJDATA_POINTERS \ - X ( mjtNum, qpos, nq, 1 ) \ - X ( mjtNum, qvel, nv, 1 ) \ - XMJV( mjtNum, act, na, 1 ) \ - X ( mjtNum, qacc_warmstart, nv, 1 ) \ - X ( mjtNum, plugin_state, npluginstate, 1 ) \ - XMJV( mjtNum, ctrl, nu, 1 ) \ - X ( mjtNum, qfrc_applied, nv, 1 ) \ - XMJV( mjtNum, xfrc_applied, nbody, 6 ) \ - XMJV( mjtByte, eq_active, neq, 1 ) \ - X ( mjtNum, mocap_pos, nmocap, 3 ) \ - X ( mjtNum, mocap_quat, nmocap, 4 ) \ - X ( mjtNum, qacc, nv, 1 ) \ - X ( mjtNum, act_dot, na, 1 ) \ - X ( mjtNum, userdata, nuserdata, 1 ) \ - XMJV( mjtNum, sensordata, nsensordata, 1 ) \ - X ( int, plugin, nplugin, 1 ) \ - X ( uintptr_t, plugin_data, nplugin, 1 ) \ - XMJV( mjtNum, xpos, nbody, 3 ) \ - XMJV( mjtNum, xquat, nbody, 4 ) \ - XMJV( mjtNum, xmat, nbody, 9 ) \ - XMJV( mjtNum, xipos, nbody, 3 ) \ - XMJV( mjtNum, ximat, nbody, 9 ) \ - XMJV( mjtNum, xanchor, njnt, 3 ) \ - XMJV( mjtNum, xaxis, njnt, 3 ) \ - XMJV( mjtNum, geom_xpos, ngeom, 3 ) \ - XMJV( mjtNum, geom_xmat, ngeom, 9 ) \ - XMJV( mjtNum, site_xpos, nsite, 3 ) \ - XMJV( mjtNum, site_xmat, nsite, 9 ) \ - XMJV( mjtNum, cam_xpos, ncam, 3 ) \ - XMJV( mjtNum, cam_xmat, ncam, 9 ) \ - XMJV( mjtNum, light_xpos, nlight, 3 ) \ - XMJV( mjtNum, light_xdir, nlight, 3 ) \ - XMJV( mjtNum, subtree_com, nbody, 3 ) \ - X ( mjtNum, cdof, nv, 6 ) \ - X ( mjtNum, cinert, nbody, 10 ) \ - XMJV( mjtNum, flexvert_xpos, nflexvert, 3 ) \ - X ( mjtNum, flexelem_aabb, nflexelem, 6 ) \ - X ( int, flexedge_J_rownnz, nflexedge, 1 ) \ - X ( int, flexedge_J_rowadr, nflexedge, 1 ) \ - X ( int, flexedge_J_colind, nflexedge, MJ_M(nv) ) \ - X ( mjtNum, flexedge_J, nflexedge, MJ_M(nv) ) \ - X ( mjtNum, flexedge_length, nflexedge, 1 ) \ - XMJV( int, ten_wrapadr, ntendon, 1 ) \ - XMJV( int, ten_wrapnum, ntendon, 1 ) \ - X ( int, ten_J_rownnz, ntendon, 1 ) \ - X ( int, ten_J_rowadr, ntendon, 1 ) \ - X ( int, ten_J_colind, ntendon, MJ_M(nv) ) \ - XMJV( mjtNum, ten_length, ntendon, 1 ) \ - X ( mjtNum, ten_J, ntendon, MJ_M(nv) ) \ - XMJV( int, wrap_obj, nwrap, 2 ) \ - XMJV( mjtNum, wrap_xpos, nwrap, 6 ) \ - X ( mjtNum, actuator_length, nu, 1 ) \ - X ( mjtNum, actuator_moment, nu, MJ_M(nv) ) \ - X ( mjtNum, crb, nbody, 10 ) \ - X ( mjtNum, qM, nM, 1 ) \ - X ( mjtNum, qLD, nM, 1 ) \ - X ( mjtNum, qLDiagInv, nv, 1 ) \ - X ( mjtNum, qLDiagSqrtInv, nv, 1 ) \ - XMJV( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ - XMJV( mjtByte, bvh_active, nbvh, 1 ) \ - X ( mjtNum, flexedge_velocity, nflexedge, 1 ) \ - X ( mjtNum, ten_velocity, ntendon, 1 ) \ - X ( mjtNum, actuator_velocity, nu, 1 ) \ - X ( mjtNum, cvel, nbody, 6 ) \ - X ( mjtNum, cdof_dot, nv, 6 ) \ - X ( mjtNum, qfrc_bias, nv, 1 ) \ - X ( mjtNum, qfrc_spring, nv, 1 ) \ - X ( mjtNum, qfrc_damper, nv, 1 ) \ - X ( mjtNum, qfrc_gravcomp, nv, 1 ) \ - X ( mjtNum, qfrc_fluid, nv, 1 ) \ - X ( mjtNum, qfrc_passive, nv, 1 ) \ - X ( mjtNum, subtree_linvel, nbody, 3 ) \ - X ( mjtNum, subtree_angmom, nbody, 3 ) \ - X ( mjtNum, qH, nM, 1 ) \ - X ( mjtNum, qHDiagInv, nv, 1 ) \ - X ( int, D_rownnz, nv, 1 ) \ - X ( int, D_rowadr, nv, 1 ) \ - X ( int, D_colind, nD, 1 ) \ - X ( int, B_rownnz, nbody, 1 ) \ - X ( int, B_rowadr, nbody, 1 ) \ - X ( int, B_colind, nB, 1 ) \ - X ( mjtNum, qDeriv, nD, 1 ) \ - X ( mjtNum, qLU, nD, 1 ) \ - X ( mjtNum, actuator_force, nu, 1 ) \ - X ( mjtNum, qfrc_actuator, nv, 1 ) \ - X ( mjtNum, qfrc_smooth, nv, 1 ) \ - X ( mjtNum, qacc_smooth, nv, 1 ) \ - X ( mjtNum, qfrc_constraint, nv, 1 ) \ - X ( mjtNum, qfrc_inverse, nv, 1 ) \ - X ( mjtNum, cacc, nbody, 6 ) \ - X ( mjtNum, cfrc_int, nbody, 6 ) \ - X ( mjtNum, cfrc_ext, nbody, 6 ) - +#define MJDATA_POINTERS \ + X(mjtNum, qpos, nq, 1) \ + X(mjtNum, qvel, nv, 1) \ + XMJV(mjtNum, act, na, 1) \ + X(mjtNum, qacc_warmstart, nv, 1) \ + X(mjtNum, plugin_state, npluginstate, 1) \ + XMJV(mjtNum, ctrl, nu, 1) \ + X(mjtNum, qfrc_applied, nv, 1) \ + XMJV(mjtNum, xfrc_applied, nbody, 6) \ + XMJV(mjtByte, eq_active, neq, 1) \ + X(mjtNum, mocap_pos, nmocap, 3) \ + X(mjtNum, mocap_quat, nmocap, 4) \ + X(mjtNum, qacc, nv, 1) \ + X(mjtNum, act_dot, na, 1) \ + X(mjtNum, userdata, nuserdata, 1) \ + XMJV(mjtNum, sensordata, nsensordata, 1) \ + X(int, plugin, nplugin, 1) \ + X(uintptr_t, plugin_data, nplugin, 1) \ + XMJV(mjtNum, xpos, nbody, 3) \ + XMJV(mjtNum, xquat, nbody, 4) \ + XMJV(mjtNum, xmat, nbody, 9) \ + XMJV(mjtNum, xipos, nbody, 3) \ + XMJV(mjtNum, ximat, nbody, 9) \ + XMJV(mjtNum, xanchor, njnt, 3) \ + XMJV(mjtNum, xaxis, njnt, 3) \ + XMJV(mjtNum, geom_xpos, ngeom, 3) \ + XMJV(mjtNum, geom_xmat, ngeom, 9) \ + XMJV(mjtNum, site_xpos, nsite, 3) \ + XMJV(mjtNum, site_xmat, nsite, 9) \ + XMJV(mjtNum, cam_xpos, ncam, 3) \ + XMJV(mjtNum, cam_xmat, ncam, 9) \ + XMJV(mjtNum, light_xpos, nlight, 3) \ + XMJV(mjtNum, light_xdir, nlight, 3) \ + XMJV(mjtNum, subtree_com, nbody, 3) \ + X(mjtNum, cdof, nv, 6) \ + X(mjtNum, cinert, nbody, 10) \ + XMJV(mjtNum, flexvert_xpos, nflexvert, 3) \ + X(mjtNum, flexelem_aabb, nflexelem, 6) \ + X(int, flexedge_J_rownnz, nflexedge, 1) \ + X(int, flexedge_J_rowadr, nflexedge, 1) \ + X(int, flexedge_J_colind, nflexedge, MJ_M(nv)) \ + X(mjtNum, flexedge_J, nflexedge, MJ_M(nv)) \ + X(mjtNum, flexedge_length, nflexedge, 1) \ + XMJV(int, ten_wrapadr, ntendon, 1) \ + XMJV(int, ten_wrapnum, ntendon, 1) \ + X(int, ten_J_rownnz, ntendon, 1) \ + X(int, ten_J_rowadr, ntendon, 1) \ + X(int, ten_J_colind, ntendon, MJ_M(nv)) \ + XMJV(mjtNum, ten_length, ntendon, 1) \ + X(mjtNum, ten_J, ntendon, MJ_M(nv)) \ + XMJV(int, wrap_obj, nwrap, 2) \ + XMJV(mjtNum, wrap_xpos, nwrap, 6) \ + X(mjtNum, actuator_length, nu, 1) \ + X(mjtNum, actuator_moment, nu, MJ_M(nv)) \ + X(mjtNum, crb, nbody, 10) \ + X(mjtNum, qM, nM, 1) \ + X(mjtNum, qLD, nM, 1) \ + X(mjtNum, qLDiagInv, nv, 1) \ + X(mjtNum, qLDiagSqrtInv, nv, 1) \ + XMJV(mjtNum, bvh_aabb_dyn, nbvhdynamic, 6) \ + XMJV(mjtByte, bvh_active, nbvh, 1) \ + X(mjtNum, flexedge_velocity, nflexedge, 1) \ + X(mjtNum, ten_velocity, ntendon, 1) \ + X(mjtNum, actuator_velocity, nu, 1) \ + X(mjtNum, cvel, nbody, 6) \ + X(mjtNum, cdof_dot, nv, 6) \ + X(mjtNum, qfrc_bias, nv, 1) \ + X(mjtNum, qfrc_spring, nv, 1) \ + X(mjtNum, qfrc_damper, nv, 1) \ + X(mjtNum, qfrc_gravcomp, nv, 1) \ + X(mjtNum, qfrc_fluid, nv, 1) \ + X(mjtNum, qfrc_passive, nv, 1) \ + X(mjtNum, subtree_linvel, nbody, 3) \ + X(mjtNum, subtree_angmom, nbody, 3) \ + X(mjtNum, qH, nM, 1) \ + X(mjtNum, qHDiagInv, nv, 1) \ + X(int, D_rownnz, nv, 1) \ + X(int, D_rowadr, nv, 1) \ + X(int, D_colind, nD, 1) \ + X(int, B_rownnz, nbody, 1) \ + X(int, B_rowadr, nbody, 1) \ + X(int, B_colind, nB, 1) \ + X(mjtNum, qDeriv, nD, 1) \ + X(mjtNum, qLU, nD, 1) \ + X(mjtNum, actuator_force, nu, 1) \ + X(mjtNum, qfrc_actuator, nv, 1) \ + X(mjtNum, qfrc_smooth, nv, 1) \ + X(mjtNum, qacc_smooth, nv, 1) \ + X(mjtNum, qfrc_constraint, nv, 1) \ + X(mjtNum, qfrc_inverse, nv, 1) \ + X(mjtNum, cacc, nbody, 6) \ + X(mjtNum, cfrc_int, nbody, 6) \ + X(mjtNum, cfrc_ext, nbody, 6) // macro for annotating that an array size in an X macro is a member of mjData // by default this macro does nothing, but users can redefine it as necessary #define MJ_D(n) n // array of contacts -#define MJDATA_ARENA_POINTERS_CONTACT \ - X( mjContact, contact, MJ_D(ncon), 1 ) +#define MJDATA_ARENA_POINTERS_CONTACT X(mjContact, contact, MJ_D(ncon), 1) // array fields of mjData that are used in the primal problem -#define MJDATA_ARENA_POINTERS_PRIMAL \ - X( int, efc_type, MJ_D(nefc), 1 ) \ - X( int, efc_id, MJ_D(nefc), 1 ) \ - X( int, efc_J_rownnz, MJ_D(nefc), 1 ) \ - X( int, efc_J_rowadr, MJ_D(nefc), 1 ) \ - X( int, efc_J_rowsuper, MJ_D(nefc), 1 ) \ - X( int, efc_J_colind, MJ_D(nnzJ), 1 ) \ - X( int, efc_JT_rownnz, MJ_M(nv), 1 ) \ - X( int, efc_JT_rowadr, MJ_M(nv), 1 ) \ - X( int, efc_JT_rowsuper, MJ_M(nv), 1 ) \ - X( int, efc_JT_colind, MJ_D(nnzJ), 1 ) \ - X( mjtNum, efc_J, MJ_D(nnzJ), 1 ) \ - X( mjtNum, efc_JT, MJ_D(nnzJ), 1 ) \ - X( mjtNum, efc_pos, MJ_D(nefc), 1 ) \ - X( mjtNum, efc_margin, MJ_D(nefc), 1 ) \ - X( mjtNum, efc_frictionloss, MJ_D(nefc), 1 ) \ - X( mjtNum, efc_diagApprox, MJ_D(nefc), 1 ) \ - X( mjtNum, efc_KBIP, MJ_D(nefc), 4 ) \ - X( mjtNum, efc_D, MJ_D(nefc), 1 ) \ - X( mjtNum, efc_R, MJ_D(nefc), 1 ) \ - X( int, tendon_efcadr, MJ_M(ntendon), 1 ) \ - X( mjtNum, efc_vel, MJ_D(nefc), 1 ) \ - X( mjtNum, efc_aref, MJ_D(nefc), 1 ) \ - X( mjtNum, efc_b, MJ_D(nefc), 1 ) \ - X( mjtNum, efc_force, MJ_D(nefc), 1 ) \ - X( int, efc_state, MJ_D(nefc), 1 ) +#define MJDATA_ARENA_POINTERS_PRIMAL \ + X(int, efc_type, MJ_D(nefc), 1) \ + X(int, efc_id, MJ_D(nefc), 1) \ + X(int, efc_J_rownnz, MJ_D(nefc), 1) \ + X(int, efc_J_rowadr, MJ_D(nefc), 1) \ + X(int, efc_J_rowsuper, MJ_D(nefc), 1) \ + X(int, efc_J_colind, MJ_D(nnzJ), 1) \ + X(int, efc_JT_rownnz, MJ_M(nv), 1) \ + X(int, efc_JT_rowadr, MJ_M(nv), 1) \ + X(int, efc_JT_rowsuper, MJ_M(nv), 1) \ + X(int, efc_JT_colind, MJ_D(nnzJ), 1) \ + X(mjtNum, efc_J, MJ_D(nnzJ), 1) \ + X(mjtNum, efc_JT, MJ_D(nnzJ), 1) \ + X(mjtNum, efc_pos, MJ_D(nefc), 1) \ + X(mjtNum, efc_margin, MJ_D(nefc), 1) \ + X(mjtNum, efc_frictionloss, MJ_D(nefc), 1) \ + X(mjtNum, efc_diagApprox, MJ_D(nefc), 1) \ + X(mjtNum, efc_KBIP, MJ_D(nefc), 4) \ + X(mjtNum, efc_D, MJ_D(nefc), 1) \ + X(mjtNum, efc_R, MJ_D(nefc), 1) \ + X(int, tendon_efcadr, MJ_M(ntendon), 1) \ + X(mjtNum, efc_vel, MJ_D(nefc), 1) \ + X(mjtNum, efc_aref, MJ_D(nefc), 1) \ + X(mjtNum, efc_b, MJ_D(nefc), 1) \ + X(mjtNum, efc_force, MJ_D(nefc), 1) \ + X(int, efc_state, MJ_D(nefc), 1) // array fields of mjData that are used in the dual problem -#define MJDATA_ARENA_POINTERS_DUAL \ - X( int, efc_AR_rownnz, MJ_D(nefc), 1 ) \ - X( int, efc_AR_rowadr, MJ_D(nefc), 1 ) \ - X( int, efc_AR_colind, MJ_D(nefc), MJ_D(nefc) ) \ - X( mjtNum, efc_AR, MJ_D(nefc), MJ_D(nefc) ) +#define MJDATA_ARENA_POINTERS_DUAL \ + X(int, efc_AR_rownnz, MJ_D(nefc), 1) \ + X(int, efc_AR_rowadr, MJ_D(nefc), 1) \ + X(int, efc_AR_colind, MJ_D(nefc), MJ_D(nefc)) \ + X(mjtNum, efc_AR, MJ_D(nefc), MJ_D(nefc)) // array fields of mjData that are used for constraint islands -#define MJDATA_ARENA_POINTERS_ISLAND \ - X( int, dof_island, MJ_M(nv), 1 ) \ - X( int, island_dofnum, MJ_D(nisland), 1 ) \ - X( int, island_dofadr, MJ_D(nisland), 1 ) \ - X( int, island_dofind, MJ_M(nv), 1 ) \ - X( int, dof_islandind, MJ_M(nv), 1 ) \ - X( int, efc_island, MJ_D(nefc), 1 ) \ - X( int, island_efcnum, MJ_D(nisland), 1 ) \ - X( int, island_efcadr, MJ_D(nisland), 1 ) \ - X( int, island_efcind, MJ_D(nefc), 1 ) +#define MJDATA_ARENA_POINTERS_ISLAND \ + X(int, dof_island, MJ_M(nv), 1) \ + X(int, island_dofnum, MJ_D(nisland), 1) \ + X(int, island_dofadr, MJ_D(nisland), 1) \ + X(int, island_dofind, MJ_M(nv), 1) \ + X(int, dof_islandind, MJ_M(nv), 1) \ + X(int, efc_island, MJ_D(nefc), 1) \ + X(int, island_efcnum, MJ_D(nisland), 1) \ + X(int, island_efcadr, MJ_D(nisland), 1) \ + X(int, island_efcind, MJ_D(nefc), 1) // array fields of mjData that live in d->arena -#define MJDATA_ARENA_POINTERS \ - MJDATA_ARENA_POINTERS_CONTACT \ - MJDATA_ARENA_POINTERS_PRIMAL \ - MJDATA_ARENA_POINTERS_DUAL \ - MJDATA_ARENA_POINTERS_ISLAND - +#define MJDATA_ARENA_POINTERS \ + MJDATA_ARENA_POINTERS_CONTACT \ + MJDATA_ARENA_POINTERS_PRIMAL \ + MJDATA_ARENA_POINTERS_DUAL \ + MJDATA_ARENA_POINTERS_ISLAND // scalar fields of mjData -#define MJDATA_SCALAR \ - X( size_t, narena ) \ - X( size_t, nbuffer ) \ - X( int, nplugin ) \ - X( size_t, pstack ) \ - X( size_t, pbase ) \ - X( size_t, parena ) \ - X( size_t, maxuse_stack ) \ - X( size_t, maxuse_arena ) \ - X( int, maxuse_con ) \ - X( int, maxuse_efc ) \ - X( int, solver_nisland ) \ - X( int, ne ) \ - X( int, nf ) \ - X( int, nl ) \ - X( int, nefc ) \ - X( int, nnzJ ) \ - X( int, ncon ) \ - X( int, nisland ) \ - X( mjtNum, time ) \ - X( uintptr_t, threadpool ) - +#define MJDATA_SCALAR \ + X(size_t, narena) \ + X(size_t, nbuffer) \ + X(int, nplugin) \ + X(size_t, pstack) \ + X(size_t, pbase) \ + X(size_t, parena) \ + X(size_t, maxuse_stack) \ + X(size_t, maxuse_arena) \ + X(int, maxuse_con) \ + X(int, maxuse_efc) \ + X(int, solver_nisland) \ + X(int, ne) \ + X(int, nf) \ + X(int, nl) \ + X(int, nefc) \ + X(int, nnzJ) \ + X(int, ncon) \ + X(int, nisland) \ + X(mjtNum, time) \ + X(uintptr_t, threadpool) // vector fields of mjData -#define MJDATA_VECTOR \ - X( size_t, maxuse_threadstack, mjMAXTHREAD, 1 ) \ - X( mjWarningStat, warning, mjNWARNING, 1 ) \ - X( mjTimerStat, timer, mjNTIMER, 1 ) \ - X( mjSolverStat, solver, mjNILSAND, mjNSOLVER ) \ - X( int, solver_niter, mjNISLAND, 1 ) \ - X( int, solver_nnz, mjNISLAND, 1 ) \ - X( mjtNum, solver_fwdinv, 2, 1 ) \ - X( mjtNum, energy, 2, 1 ) - +#define MJDATA_VECTOR \ + X(size_t, maxuse_threadstack, mjMAXTHREAD, 1) \ + X(mjWarningStat, warning, mjNWARNING, 1) \ + X(mjTimerStat, timer, mjNTIMER, 1) \ + X(mjSolverStat, solver, mjNILSAND, mjNSOLVER) \ + X(int, solver_niter, mjNISLAND, 1) \ + X(int, solver_nnz, mjNISLAND, 1) \ + X(mjtNum, solver_fwdinv, 2, 1) \ + X(mjtNum, energy, 2, 1) // alias XMJV to be the same as X // to obtain only X macros for fields that are relevant for mjvScene creation, // redefine X to expand to nothing, and XMJV to do what's required #define XMJV X -#endif // MUJOCO_MJXMACRO_H_ +#endif // MUJOCO_MJXMACRO_H_ diff --git a/mujoco/include/mujoco/mujoco.h b/mujoco/include/mujoco/mujoco.h index 3d7c6429..79d04955 100644 --- a/mujoco/include/mujoco/mujoco.h +++ b/mujoco/include/mujoco/mujoco.h @@ -17,7 +17,6 @@ #include - // this is a C-API #ifdef __cplusplus extern "C" { @@ -27,13 +26,13 @@ extern "C" { #define mjVERSION_HEADER 314 // needed to define size_t, fabs and log10 -#include #include +#include // type definitions #include -#include #include +#include #include #include #include @@ -41,739 +40,814 @@ extern "C" { #include #include - // user error and memory handlers -MJAPI extern void (*mju_user_error)(const char*); -MJAPI extern void (*mju_user_warning)(const char*); -MJAPI extern void* (*mju_user_malloc)(size_t); -MJAPI extern void (*mju_user_free)(void*); - +MJAPI extern void (*mju_user_error)(const char *); +MJAPI extern void (*mju_user_warning)(const char *); +MJAPI extern void *(*mju_user_malloc)(size_t); +MJAPI extern void (*mju_user_free)(void *); // callbacks extending computation pipeline -MJAPI extern mjfGeneric mjcb_passive; -MJAPI extern mjfGeneric mjcb_control; -MJAPI extern mjfConFilt mjcb_contactfilter; -MJAPI extern mjfSensor mjcb_sensor; -MJAPI extern mjfTime mjcb_time; -MJAPI extern mjfAct mjcb_act_dyn; -MJAPI extern mjfAct mjcb_act_gain; -MJAPI extern mjfAct mjcb_act_bias; - +MJAPI extern mjfGeneric mjcb_passive; +MJAPI extern mjfGeneric mjcb_control; +MJAPI extern mjfConFilt mjcb_contactfilter; +MJAPI extern mjfSensor mjcb_sensor; +MJAPI extern mjfTime mjcb_time; +MJAPI extern mjfAct mjcb_act_dyn; +MJAPI extern mjfAct mjcb_act_gain; +MJAPI extern mjfAct mjcb_act_bias; // collision function table MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES]; - // string names -MJAPI extern const char* mjDISABLESTRING[mjNDISABLE]; -MJAPI extern const char* mjENABLESTRING[mjNENABLE]; -MJAPI extern const char* mjTIMERSTRING[mjNTIMER]; -MJAPI extern const char* mjLABELSTRING[mjNLABEL]; -MJAPI extern const char* mjFRAMESTRING[mjNFRAME]; -MJAPI extern const char* mjVISSTRING[mjNVISFLAG][3]; -MJAPI extern const char* mjRNDSTRING[mjNRNDFLAG][3]; - +MJAPI extern const char *mjDISABLESTRING[mjNDISABLE]; +MJAPI extern const char *mjENABLESTRING[mjNENABLE]; +MJAPI extern const char *mjTIMERSTRING[mjNTIMER]; +MJAPI extern const char *mjLABELSTRING[mjNLABEL]; +MJAPI extern const char *mjFRAMESTRING[mjNFRAME]; +MJAPI extern const char *mjVISSTRING[mjNVISFLAG][3]; +MJAPI extern const char *mjRNDSTRING[mjNRNDFLAG][3]; -//---------------------------------- Virtual file system ------------------------------------------- +//---------------------------------- Virtual file system +//------------------------------------------- // Initialize VFS to empty (no deallocation). -MJAPI void mj_defaultVFS(mjVFS* vfs); +MJAPI void mj_defaultVFS(mjVFS *vfs); -// Add file to VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load. -MJAPI int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); +// Add file to VFS, return 0: success, 1: full, 2: repeated name, -1: failed to +// load. +MJAPI int mj_addFileVFS(mjVFS *vfs, const char *directory, + const char *filename); -// Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load. -MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer); +// Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, +// -1: failed to load. +MJAPI int mj_addBufferVFS(mjVFS *vfs, const char *name, const void *buffer, + int nbuffer); // Return file index in VFS, or -1 if not found in VFS. -MJAPI int mj_findFileVFS(const mjVFS* vfs, const char* filename); +MJAPI int mj_findFileVFS(const mjVFS *vfs, const char *filename); // Delete file from VFS, return 0: success, -1: not found in VFS. -MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename); +MJAPI int mj_deleteFileVFS(mjVFS *vfs, const char *filename); // Delete all files from VFS. -MJAPI void mj_deleteVFS(mjVFS* vfs); +MJAPI void mj_deleteVFS(mjVFS *vfs); // deprecated: use mj_copyBufferVFS. -MJAPI int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize); +MJAPI int mj_makeEmptyFileVFS(mjVFS *vfs, const char *filename, int filesize); -//---------------------------------- Parse and compile --------------------------------------------- +//---------------------------------- Parse and compile +//--------------------------------------------- // Parse XML file in MJCF or URDF format, compile it, return low-level model. // If vfs is not NULL, look up files in vfs before reading from disk. // If error is not NULL, it must have size error_sz. -MJAPI mjModel* mj_loadXML(const char* filename, const mjVFS* vfs, char* error, int error_sz); +MJAPI mjModel *mj_loadXML(const char *filename, const mjVFS *vfs, char *error, + int error_sz); // Update XML data structures with info from low-level model, save as MJCF. // If error is not NULL, it must have size error_sz. -MJAPI int mj_saveLastXML(const char* filename, const mjModel* m, char* error, int error_sz); +MJAPI int mj_saveLastXML(const char *filename, const mjModel *m, char *error, + int error_sz); // Free last XML model if loaded. Called internally at each load. MJAPI void mj_freeLastXML(void); -// Print internal XML schema as plain text or HTML, with style-padding or  . -MJAPI int mj_printSchema(const char* filename, char* buffer, int buffer_sz, +// Print internal XML schema as plain text or HTML, with style-padding or +//  . +MJAPI int mj_printSchema(const char *filename, char *buffer, int buffer_sz, int flg_html, int flg_pad); +//---------------------------------- Main simulation +//----------------------------------------------- -//---------------------------------- Main simulation ----------------------------------------------- +// Advance simulation, use control callback to obtain external force and +// control. +MJAPI void mj_step(const mjModel *m, mjData *d); -// Advance simulation, use control callback to obtain external force and control. -MJAPI void mj_step(const mjModel* m, mjData* d); +// Advance simulation in two steps: before external force and control is set by +// user. +MJAPI void mj_step1(const mjModel *m, mjData *d); -// Advance simulation in two steps: before external force and control is set by user. -MJAPI void mj_step1(const mjModel* m, mjData* d); - -// Advance simulation in two steps: after external force and control is set by user. -MJAPI void mj_step2(const mjModel* m, mjData* d); +// Advance simulation in two steps: after external force and control is set by +// user. +MJAPI void mj_step2(const mjModel *m, mjData *d); // Forward dynamics: same as mj_step but do not integrate in time. -MJAPI void mj_forward(const mjModel* m, mjData* d); +MJAPI void mj_forward(const mjModel *m, mjData *d); // Inverse dynamics: qacc must be set before calling. -MJAPI void mj_inverse(const mjModel* m, mjData* d); +MJAPI void mj_inverse(const mjModel *m, mjData *d); // Forward dynamics with skip; skipstage is mjtStage. -MJAPI void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor); +MJAPI void mj_forwardSkip(const mjModel *m, mjData *d, int skipstage, + int skipsensor); // Inverse dynamics with skip; skipstage is mjtStage. -MJAPI void mj_inverseSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor); - +MJAPI void mj_inverseSkip(const mjModel *m, mjData *d, int skipstage, + int skipsensor); -//---------------------------------- Initialization ------------------------------------------------ +//---------------------------------- Initialization +//------------------------------------------------ // Set default options for length range computation. -MJAPI void mj_defaultLROpt(mjLROpt* opt); +MJAPI void mj_defaultLROpt(mjLROpt *opt); // Set solver parameters to default values. -MJAPI void mj_defaultSolRefImp(mjtNum* solref, mjtNum* solimp); +MJAPI void mj_defaultSolRefImp(mjtNum *solref, mjtNum *solimp); // Set physics options to default values. -MJAPI void mj_defaultOption(mjOption* opt); +MJAPI void mj_defaultOption(mjOption *opt); // Set visual options to default values. -MJAPI void mj_defaultVisual(mjVisual* vis); +MJAPI void mj_defaultVisual(mjVisual *vis); // Copy mjModel, allocate new if dest is NULL. -MJAPI mjModel* mj_copyModel(mjModel* dest, const mjModel* src); +MJAPI mjModel *mj_copyModel(mjModel *dest, const mjModel *src); -// Save model to binary MJB file or memory buffer; buffer has precedence when given. -MJAPI void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buffer_sz); +// Save model to binary MJB file or memory buffer; buffer has precedence when +// given. +MJAPI void mj_saveModel(const mjModel *m, const char *filename, void *buffer, + int buffer_sz); // Load model from binary MJB file. // If vfs is not NULL, look up file in vfs before reading from disk. -MJAPI mjModel* mj_loadModel(const char* filename, const mjVFS* vfs); +MJAPI mjModel *mj_loadModel(const char *filename, const mjVFS *vfs); // Free memory allocation in model. -MJAPI void mj_deleteModel(mjModel* m); +MJAPI void mj_deleteModel(mjModel *m); // Return size of buffer needed to hold model. -MJAPI int mj_sizeModel(const mjModel* m); +MJAPI int mj_sizeModel(const mjModel *m); // Allocate mjData corresponding to given model. // If the model buffer is unallocated the initial configuration will not be set. -MJAPI mjData* mj_makeData(const mjModel* m); +MJAPI mjData *mj_makeData(const mjModel *m); // Copy mjData. // m is only required to contain the size fields from MJMODEL_INTS. -MJAPI mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src); +MJAPI mjData *mj_copyData(mjData *dest, const mjModel *m, const mjData *src); // Reset data to defaults. -MJAPI void mj_resetData(const mjModel* m, mjData* d); +MJAPI void mj_resetData(const mjModel *m, mjData *d); // Reset data to defaults, fill everything else with debug_value. -MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value); +MJAPI void mj_resetDataDebug(const mjModel *m, mjData *d, + unsigned char debug_value); // Reset data. If 0 <= key < nkey, set fields from specified keyframe. -MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key); +MJAPI void mj_resetDataKeyframe(const mjModel *m, mjData *d, int key); #ifndef ADDRESS_SANITIZER // Mark a new frame on the mjData stack. -MJAPI void mj_markStack(mjData* d); +MJAPI void mj_markStack(mjData *d); -// Free the current mjData stack frame. All pointers returned by mj_stackAlloc since the last call -// to mj_markStack must no longer be used afterwards. -MJAPI void mj_freeStack(mjData* d); +// Free the current mjData stack frame. All pointers returned by mj_stackAlloc +// since the last call to mj_markStack must no longer be used afterwards. +MJAPI void mj_freeStack(mjData *d); -#endif // ADDRESS_SANITIZER +#endif // ADDRESS_SANITIZER // Allocate a number of bytes on mjData stack at a specific alignment. // Call mju_error on stack overflow. -MJAPI void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment); +MJAPI void *mj_stackAllocByte(mjData *d, size_t bytes, size_t alignment); // Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow. -MJAPI mjtNum* mj_stackAllocNum(mjData* d, int size); +MJAPI mjtNum *mj_stackAllocNum(mjData *d, int size); // Allocate array of ints on mjData stack. Call mju_error on stack overflow. -MJAPI int* mj_stackAllocInt(mjData* d, int size); +MJAPI int *mj_stackAllocInt(mjData *d, int size); // Free memory allocation in mjData. -MJAPI void mj_deleteData(mjData* d); +MJAPI void mj_deleteData(mjData *d); // Reset all callbacks to NULL pointers (NULL is the default). MJAPI void mj_resetCallbacks(void); // Set constant fields of mjModel, corresponding to qpos0 configuration. -MJAPI void mj_setConst(mjModel* m, mjData* d); +MJAPI void mj_setConst(mjModel *m, mjData *d); // Set actuator_lengthrange for specified actuator; return 1 if ok, 0 if error. -MJAPI int mj_setLengthRange(mjModel* m, mjData* d, int index, - const mjLROpt* opt, char* error, int error_sz); +MJAPI int mj_setLengthRange(mjModel *m, mjData *d, int index, + const mjLROpt *opt, char *error, int error_sz); - -//---------------------------------- Printing ------------------------------------------------------ +//---------------------------------- Printing +//------------------------------------------------------ // Print mjModel to text file, specifying format. -// float_format must be a valid printf-style format string for a single float value. -MJAPI void mj_printFormattedModel(const mjModel* m, const char* filename, const char* float_format); +// float_format must be a valid printf-style format string for a single float +// value. +MJAPI void mj_printFormattedModel(const mjModel *m, const char *filename, + const char *float_format); // Print model to text file. -MJAPI void mj_printModel(const mjModel* m, const char* filename); +MJAPI void mj_printModel(const mjModel *m, const char *filename); // Print mjData to text file, specifying format. -// float_format must be a valid printf-style format string for a single float value -MJAPI void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, - const char* float_format); +// float_format must be a valid printf-style format string for a single float +// value +MJAPI void mj_printFormattedData(const mjModel *m, mjData *d, + const char *filename, + const char *float_format); // Print data to text file. -MJAPI void mj_printData(const mjModel* m, mjData* d, const char* filename); +MJAPI void mj_printData(const mjModel *m, mjData *d, const char *filename); // Print matrix to screen. -MJAPI void mju_printMat(const mjtNum* mat, int nr, int nc); +MJAPI void mju_printMat(const mjtNum *mat, int nr, int nc); // Print sparse matrix to screen. -MJAPI void mju_printMatSparse(const mjtNum* mat, int nr, - const int* rownnz, const int* rowadr, const int* colind); - +MJAPI void mju_printMatSparse(const mjtNum *mat, int nr, const int *rownnz, + const int *rowadr, const int *colind); -//---------------------------------- Components ---------------------------------------------------- +//---------------------------------- Components +//---------------------------------------------------- // Run position-dependent computations. -MJAPI void mj_fwdPosition(const mjModel* m, mjData* d); +MJAPI void mj_fwdPosition(const mjModel *m, mjData *d); // Run velocity-dependent computations. -MJAPI void mj_fwdVelocity(const mjModel* m, mjData* d); +MJAPI void mj_fwdVelocity(const mjModel *m, mjData *d); // Compute actuator force qfrc_actuator. -MJAPI void mj_fwdActuation(const mjModel* m, mjData* d); +MJAPI void mj_fwdActuation(const mjModel *m, mjData *d); // Add up all non-constraint forces, compute qacc_smooth. -MJAPI void mj_fwdAcceleration(const mjModel* m, mjData* d); +MJAPI void mj_fwdAcceleration(const mjModel *m, mjData *d); // Run selected constraint solver. -MJAPI void mj_fwdConstraint(const mjModel* m, mjData* d); +MJAPI void mj_fwdConstraint(const mjModel *m, mjData *d); // Euler integrator, semi-implicit in velocity. -MJAPI void mj_Euler(const mjModel* m, mjData* d); +MJAPI void mj_Euler(const mjModel *m, mjData *d); // Runge-Kutta explicit order-N integrator. -MJAPI void mj_RungeKutta(const mjModel* m, mjData* d, int N); +MJAPI void mj_RungeKutta(const mjModel *m, mjData *d, int N); // Implicit-in-velocity integrators. -MJAPI void mj_implicit(const mjModel* m, mjData* d); +MJAPI void mj_implicit(const mjModel *m, mjData *d); // Run position-dependent computations in inverse dynamics. -MJAPI void mj_invPosition(const mjModel* m, mjData* d); +MJAPI void mj_invPosition(const mjModel *m, mjData *d); // Run velocity-dependent computations in inverse dynamics. -MJAPI void mj_invVelocity(const mjModel* m, mjData* d); +MJAPI void mj_invVelocity(const mjModel *m, mjData *d); // Apply the analytical formula for inverse constraint dynamics. -MJAPI void mj_invConstraint(const mjModel* m, mjData* d); +MJAPI void mj_invConstraint(const mjModel *m, mjData *d); // Compare forward and inverse dynamics, save results in fwdinv. -MJAPI void mj_compareFwdInv(const mjModel* m, mjData* d); - +MJAPI void mj_compareFwdInv(const mjModel *m, mjData *d); -//---------------------------------- Sub components ------------------------------------------------ +//---------------------------------- Sub components +//------------------------------------------------ // Evaluate position-dependent sensors. -MJAPI void mj_sensorPos(const mjModel* m, mjData* d); +MJAPI void mj_sensorPos(const mjModel *m, mjData *d); // Evaluate velocity-dependent sensors. -MJAPI void mj_sensorVel(const mjModel* m, mjData* d); +MJAPI void mj_sensorVel(const mjModel *m, mjData *d); // Evaluate acceleration and force-dependent sensors. -MJAPI void mj_sensorAcc(const mjModel* m, mjData* d); +MJAPI void mj_sensorAcc(const mjModel *m, mjData *d); // Evaluate position-dependent energy (potential). -MJAPI void mj_energyPos(const mjModel* m, mjData* d); +MJAPI void mj_energyPos(const mjModel *m, mjData *d); // Evaluate velocity-dependent energy (kinetic). -MJAPI void mj_energyVel(const mjModel* m, mjData* d); +MJAPI void mj_energyVel(const mjModel *m, mjData *d); // Check qpos, reset if any element is too big or nan. -MJAPI void mj_checkPos(const mjModel* m, mjData* d); +MJAPI void mj_checkPos(const mjModel *m, mjData *d); // Check qvel, reset if any element is too big or nan. -MJAPI void mj_checkVel(const mjModel* m, mjData* d); +MJAPI void mj_checkVel(const mjModel *m, mjData *d); // Check qacc, reset if any element is too big or nan. -MJAPI void mj_checkAcc(const mjModel* m, mjData* d); +MJAPI void mj_checkAcc(const mjModel *m, mjData *d); // Run forward kinematics. -MJAPI void mj_kinematics(const mjModel* m, mjData* d); +MJAPI void mj_kinematics(const mjModel *m, mjData *d); // Map inertias and motion dofs to global frame centered at CoM. -MJAPI void mj_comPos(const mjModel* m, mjData* d); +MJAPI void mj_comPos(const mjModel *m, mjData *d); // Compute camera and light positions and orientations. -MJAPI void mj_camlight(const mjModel* m, mjData* d); +MJAPI void mj_camlight(const mjModel *m, mjData *d); // Compute flex-related quantities. -MJAPI void mj_flex(const mjModel* m, mjData* d); +MJAPI void mj_flex(const mjModel *m, mjData *d); // Compute tendon lengths, velocities and moment arms. -MJAPI void mj_tendon(const mjModel* m, mjData* d); +MJAPI void mj_tendon(const mjModel *m, mjData *d); // Compute actuator transmission lengths and moments. -MJAPI void mj_transmission(const mjModel* m, mjData* d); +MJAPI void mj_transmission(const mjModel *m, mjData *d); // Run composite rigid body inertia algorithm (CRB). -MJAPI void mj_crb(const mjModel* m, mjData* d); +MJAPI void mj_crb(const mjModel *m, mjData *d); // Compute sparse L'*D*L factorizaton of inertia matrix. -MJAPI void mj_factorM(const mjModel* m, mjData* d); +MJAPI void mj_factorM(const mjModel *m, mjData *d); // Solve linear system M * x = y using factorization: x = inv(L'*D*L)*y -MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); +MJAPI void mj_solveM(const mjModel *m, mjData *d, mjtNum *x, const mjtNum *y, + int n); // Half of linear solve: x = sqrt(inv(D))*inv(L')*y -MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); +MJAPI void mj_solveM2(const mjModel *m, mjData *d, mjtNum *x, const mjtNum *y, + int n); // Compute cvel, cdof_dot. -MJAPI void mj_comVel(const mjModel* m, mjData* d); +MJAPI void mj_comVel(const mjModel *m, mjData *d); -// Compute qfrc_passive from spring-dampers, gravity compensation and fluid forces. -MJAPI void mj_passive(const mjModel* m, mjData* d); +// Compute qfrc_passive from spring-dampers, gravity compensation and fluid +// forces. +MJAPI void mj_passive(const mjModel *m, mjData *d); -// Sub-tree linear velocity and angular momentum: compute subtree_linvel, subtree_angmom. -MJAPI void mj_subtreeVel(const mjModel* m, mjData* d); +// Sub-tree linear velocity and angular momentum: compute subtree_linvel, +// subtree_angmom. +MJAPI void mj_subtreeVel(const mjModel *m, mjData *d); // RNE: compute M(qpos)*qacc + C(qpos,qvel); flg_acc=0 removes inertial term. -MJAPI void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); +MJAPI void mj_rne(const mjModel *m, mjData *d, int flg_acc, mjtNum *result); // RNE with complete data: compute cacc, cfrc_ext, cfrc_int. -MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d); +MJAPI void mj_rnePostConstraint(const mjModel *m, mjData *d); // Run collision detection. -MJAPI void mj_collision(const mjModel* m, mjData* d); +MJAPI void mj_collision(const mjModel *m, mjData *d); // Construct constraints. -MJAPI void mj_makeConstraint(const mjModel* m, mjData* d); +MJAPI void mj_makeConstraint(const mjModel *m, mjData *d); // Find constraint islands. -MJAPI void mj_island(const mjModel* m, mjData* d); +MJAPI void mj_island(const mjModel *m, mjData *d); // Compute inverse constraint inertia efc_AR. -MJAPI void mj_projectConstraint(const mjModel* m, mjData* d); +MJAPI void mj_projectConstraint(const mjModel *m, mjData *d); // Compute efc_vel, efc_aref. -MJAPI void mj_referenceConstraint(const mjModel* m, mjData* d); +MJAPI void mj_referenceConstraint(const mjModel *m, mjData *d); -// Compute efc_state, efc_force, qfrc_constraint, and (optionally) cone Hessians. -// If cost is not NULL, set *cost = s(jar) where jar = Jac*qacc-aref. -MJAPI void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, +// Compute efc_state, efc_force, qfrc_constraint, and (optionally) cone +// Hessians. If cost is not NULL, set *cost = s(jar) where jar = Jac*qacc-aref. +MJAPI void mj_constraintUpdate(const mjModel *m, mjData *d, const mjtNum *jar, mjtNum cost[1], int flg_coneHessian); - -//---------------------------------- Support ------------------------------------------------------- +//---------------------------------- Support +//------------------------------------------------------- // Return size of state specification. -MJAPI int mj_stateSize(const mjModel* m, unsigned int spec); +MJAPI int mj_stateSize(const mjModel *m, unsigned int spec); // Get state. -MJAPI void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigned int spec); +MJAPI void mj_getState(const mjModel *m, const mjData *d, mjtNum *state, + unsigned int spec); // Set state. -MJAPI void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int spec); +MJAPI void mj_setState(const mjModel *m, mjData *d, const mjtNum *state, + unsigned int spec); // Add contact to d->contact list; return 0 if success; 1 if buffer full. -MJAPI int mj_addContact(const mjModel* m, mjData* d, const mjContact* con); +MJAPI int mj_addContact(const mjModel *m, mjData *d, const mjContact *con); // Determine type of friction cone. -MJAPI int mj_isPyramidal(const mjModel* m); +MJAPI int mj_isPyramidal(const mjModel *m); // Determine type of constraint Jacobian. -MJAPI int mj_isSparse(const mjModel* m); +MJAPI int mj_isSparse(const mjModel *m); // Determine type of solver (PGS is dual, CG and Newton are primal). -MJAPI int mj_isDual(const mjModel* m); +MJAPI int mj_isDual(const mjModel *m); // Multiply dense or sparse constraint Jacobian by vector. -MJAPI void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); +MJAPI void mj_mulJacVec(const mjModel *m, const mjData *d, mjtNum *res, + const mjtNum *vec); // Multiply dense or sparse constraint Jacobian transpose by vector. -MJAPI void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); +MJAPI void mj_mulJacTVec(const mjModel *m, const mjData *d, mjtNum *res, + const mjtNum *vec); -// Compute 3/6-by-nv end-effector Jacobian of global point attached to given body. -MJAPI void mj_jac(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, +// Compute 3/6-by-nv end-effector Jacobian of global point attached to given +// body. +MJAPI void mj_jac(const mjModel *m, const mjData *d, mjtNum *jacp, mjtNum *jacr, const mjtNum point[3], int body); // Compute body frame end-effector Jacobian. -MJAPI void mj_jacBody(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body); +MJAPI void mj_jacBody(const mjModel *m, const mjData *d, mjtNum *jacp, + mjtNum *jacr, int body); // Compute body center-of-mass end-effector Jacobian. -MJAPI void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body); +MJAPI void mj_jacBodyCom(const mjModel *m, const mjData *d, mjtNum *jacp, + mjtNum *jacr, int body); // Compute subtree center-of-mass end-effector Jacobian. -MJAPI void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body); +MJAPI void mj_jacSubtreeCom(const mjModel *m, mjData *d, mjtNum *jacp, + int body); // Compute geom end-effector Jacobian. -MJAPI void mj_jacGeom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int geom); +MJAPI void mj_jacGeom(const mjModel *m, const mjData *d, mjtNum *jacp, + mjtNum *jacr, int geom); // Compute site end-effector Jacobian. -MJAPI void mj_jacSite(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int site); +MJAPI void mj_jacSite(const mjModel *m, const mjData *d, mjtNum *jacp, + mjtNum *jacr, int site); -// Compute translation end-effector Jacobian of point, and rotation Jacobian of axis. -MJAPI void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacAxis, - const mjtNum point[3], const mjtNum axis[3], int body); +// Compute translation end-effector Jacobian of point, and rotation Jacobian of +// axis. +MJAPI void mj_jacPointAxis(const mjModel *m, mjData *d, mjtNum *jacPoint, + mjtNum *jacAxis, const mjtNum point[3], + const mjtNum axis[3], int body); // Compute subtree angular momentum matrix. -MJAPI void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body); +MJAPI void mj_angmomMat(const mjModel *m, mjData *d, mjtNum *mat, int body); -// Get id of object with the specified mjtObj type and name, returns -1 if id not found. -MJAPI int mj_name2id(const mjModel* m, int type, const char* name); +// Get id of object with the specified mjtObj type and name, returns -1 if id +// not found. +MJAPI int mj_name2id(const mjModel *m, int type, const char *name); -// Get name of object with the specified mjtObj type and id, returns NULL if name not found. -MJAPI const char* mj_id2name(const mjModel* m, int type, int id); +// Get name of object with the specified mjtObj type and id, returns NULL if +// name not found. +MJAPI const char *mj_id2name(const mjModel *m, int type, int id); // Convert sparse inertia matrix M into full (i.e. dense) matrix. -MJAPI void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M); +MJAPI void mj_fullM(const mjModel *m, mjtNum *dst, const mjtNum *M); // Multiply vector by inertia matrix. -MJAPI void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); +MJAPI void mj_mulM(const mjModel *m, const mjData *d, mjtNum *res, + const mjtNum *vec); // Multiply vector by (inertia matrix)^(1/2). -MJAPI void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); +MJAPI void mj_mulM2(const mjModel *m, const mjData *d, mjtNum *res, + const mjtNum *vec); // Add inertia matrix to destination matrix. // Destination can be sparse uncompressed, or dense when all int* are NULL -MJAPI void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* rownnz, int* rowadr, int* colind); +MJAPI void mj_addM(const mjModel *m, mjData *d, mjtNum *dst, int *rownnz, + int *rowadr, int *colind); // Apply Cartesian force and torque (outside xfrc_applied mechanism). -MJAPI void mj_applyFT(const mjModel* m, mjData* d, const mjtNum force[3], const mjtNum torque[3], - const mjtNum point[3], int body, mjtNum* qfrc_target); +MJAPI void mj_applyFT(const mjModel *m, mjData *d, const mjtNum force[3], + const mjtNum torque[3], const mjtNum point[3], int body, + mjtNum *qfrc_target); -// Compute object 6D velocity (rot:lin) in object-centered frame, world/local orientation. -MJAPI void mj_objectVelocity(const mjModel* m, const mjData* d, - int objtype, int objid, mjtNum res[6], int flg_local); +// Compute object 6D velocity (rot:lin) in object-centered frame, world/local +// orientation. +MJAPI void mj_objectVelocity(const mjModel *m, const mjData *d, int objtype, + int objid, mjtNum res[6], int flg_local); -// Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation. -MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d, - int objtype, int objid, mjtNum res[6], int flg_local); +// Compute object 6D acceleration (rot:lin) in object-centered frame, +// world/local orientation. +MJAPI void mj_objectAcceleration(const mjModel *m, const mjData *d, int objtype, + int objid, mjtNum res[6], int flg_local); // Extract 6D force:torque given contact id, in the contact frame. -MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); +MJAPI void mj_contactForce(const mjModel *m, const mjData *d, int id, + mjtNum result[6]); // Compute velocity by finite-differencing two positions. -MJAPI void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt, - const mjtNum* qpos1, const mjtNum* qpos2); +MJAPI void mj_differentiatePos(const mjModel *m, mjtNum *qvel, mjtNum dt, + const mjtNum *qpos1, const mjtNum *qpos2); // Integrate position with given velocity. -MJAPI void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt); +MJAPI void mj_integratePos(const mjModel *m, mjtNum *qpos, const mjtNum *qvel, + mjtNum dt); // Normalize all quaternions in qpos-type vector. -MJAPI void mj_normalizeQuat(const mjModel* m, mjtNum* qpos); +MJAPI void mj_normalizeQuat(const mjModel *m, mjtNum *qpos); // Map from body local to global Cartesian coordinates. -MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], const mjtNum pos[3], - const mjtNum quat[4], int body, mjtByte sameframe); +MJAPI void mj_local2Global(mjData *d, mjtNum xpos[3], mjtNum xmat[9], + const mjtNum pos[3], const mjtNum quat[4], int body, + mjtByte sameframe); // Sum all body masses. -MJAPI mjtNum mj_getTotalmass(const mjModel* m); +MJAPI mjtNum mj_getTotalmass(const mjModel *m); // Scale body masses and inertias to achieve specified total mass. -MJAPI void mj_setTotalmass(mjModel* m, mjtNum newmass); +MJAPI void mj_setTotalmass(mjModel *m, mjtNum newmass); // Return a config attribute value of a plugin instance; // NULL: invalid plugin instance ID or attribute name -MJAPI const char* mj_getPluginConfig(const mjModel* m, int plugin_id, const char* attrib); +MJAPI const char *mj_getPluginConfig(const mjModel *m, int plugin_id, + const char *attrib); -// Load a dynamic library. The dynamic library is assumed to register one or more plugins. -MJAPI void mj_loadPluginLibrary(const char* path); +// Load a dynamic library. The dynamic library is assumed to register one or +// more plugins. +MJAPI void mj_loadPluginLibrary(const char *path); -// Scan a directory and load all dynamic libraries. Dynamic libraries in the specified directory -// are assumed to register one or more plugins. Optionally, if a callback is specified, it is called -// for each dynamic library encountered that registers plugins. -MJAPI void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoadCallback callback); +// Scan a directory and load all dynamic libraries. Dynamic libraries in the +// specified directory are assumed to register one or more plugins. Optionally, +// if a callback is specified, it is called for each dynamic library encountered +// that registers plugins. +MJAPI void mj_loadAllPluginLibraries(const char *directory, + mjfPluginLibraryLoadCallback callback); // Return version number: 1.0.2 is encoded as 102. MJAPI int mj_version(void); // Return the current version of MuJoCo as a null-terminated string. -MJAPI const char* mj_versionString(void); - +MJAPI const char *mj_versionString(void); -//---------------------------------- Ray collisions ------------------------------------------------ +//---------------------------------- Ray collisions +//------------------------------------------------ // Intersect multiple rays emanating from a single point. // Similar semantics to mj_ray, but vec is an array of (nray x 3) directions. -MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int* geomid, mjtNum* dist, int nray, mjtNum cutoff); - -// Intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms in bodyexclude. -// Return distance (x) to nearest surface, or -1 if no intersection and output geomid. -// geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion. -MJAPI mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int geomid[1]); +MJAPI void mj_multiRay(const mjModel *m, mjData *d, const mjtNum pnt[3], + const mjtNum *vec, const mjtByte *geomgroup, + mjtByte flg_static, int bodyexclude, int *geomid, + mjtNum *dist, int nray, mjtNum cutoff); + +// Intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms in +// bodyexclude. Return distance (x) to nearest surface, or -1 if no intersection +// and output geomid. geomgroup, flg_static are as in mjvOption; geomgroup==NULL +// skips group exclusion. +MJAPI mjtNum mj_ray(const mjModel *m, const mjData *d, const mjtNum pnt[3], + const mjtNum vec[3], const mjtByte *geomgroup, + mjtByte flg_static, int bodyexclude, int geomid[1]); // Intersect ray with hfield, return nearest distance or -1 if no intersection. -MJAPI mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid, +MJAPI mjtNum mj_rayHfield(const mjModel *m, const mjData *d, int geomid, const mjtNum pnt[3], const mjtNum vec[3]); // Intersect ray with mesh, return nearest distance or -1 if no intersection. -MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid, +MJAPI mjtNum mj_rayMesh(const mjModel *m, const mjData *d, int geomid, const mjtNum pnt[3], const mjtNum vec[3]); -// Intersect ray with pure geom, return nearest distance or -1 if no intersection. -MJAPI mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], - const mjtNum pnt[3], const mjtNum vec[3], int geomtype); +// Intersect ray with pure geom, return nearest distance or -1 if no +// intersection. +MJAPI mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], + const mjtNum size[3], const mjtNum pnt[3], + const mjtNum vec[3], int geomtype); // Intersect ray with flex, return nearest distance or -1 if no intersection, // and also output nearest vertex id. -MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert, - mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, - const mjtNum* pnt, const mjtNum* vec, int vertid[1]); +MJAPI mjtNum mju_rayFlex(const mjModel *m, const mjData *d, int flex_layer, + mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, + mjtByte flg_skin, int flexid, const mjtNum *pnt, + const mjtNum *vec, int vertid[1]); // Intersect ray with skin, return nearest distance or -1 if no intersection, // and also output nearest vertex id. -MJAPI mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, - const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]); - +MJAPI mjtNum mju_raySkin(int nface, int nvert, const int *face, + const float *vert, const mjtNum pnt[3], + const mjtNum vec[3], int vertid[1]); -//---------------------------------- Interaction --------------------------------------------------- +//---------------------------------- Interaction +//--------------------------------------------------- // Set default camera. -MJAPI void mjv_defaultCamera(mjvCamera* cam); +MJAPI void mjv_defaultCamera(mjvCamera *cam); // Set default free camera. -MJAPI void mjv_defaultFreeCamera(const mjModel* m, mjvCamera* cam); +MJAPI void mjv_defaultFreeCamera(const mjModel *m, mjvCamera *cam); // Set default perturbation. -MJAPI void mjv_defaultPerturb(mjvPerturb* pert); +MJAPI void mjv_defaultPerturb(mjvPerturb *pert); // Transform pose from room to model space. -MJAPI void mjv_room2model(mjtNum modelpos[3], mjtNum modelquat[4], const mjtNum roompos[3], - const mjtNum roomquat[4], const mjvScene* scn); +MJAPI void mjv_room2model(mjtNum modelpos[3], mjtNum modelquat[4], + const mjtNum roompos[3], const mjtNum roomquat[4], + const mjvScene *scn); // Transform pose from model to room space. -MJAPI void mjv_model2room(mjtNum roompos[3], mjtNum roomquat[4], const mjtNum modelpos[3], - const mjtNum modelquat[4], const mjvScene* scn); +MJAPI void mjv_model2room(mjtNum roompos[3], mjtNum roomquat[4], + const mjtNum modelpos[3], const mjtNum modelquat[4], + const mjvScene *scn); // Get camera info in model space; average left and right OpenGL cameras. MJAPI void mjv_cameraInModel(mjtNum headpos[3], mjtNum forward[3], mjtNum up[3], - const mjvScene* scn); + const mjvScene *scn); // Get camera info in room space; average left and right OpenGL cameras. MJAPI void mjv_cameraInRoom(mjtNum headpos[3], mjtNum forward[3], mjtNum up[3], - const mjvScene* scn); + const mjvScene *scn); -// Get frustum height at unit distance from camera; average left and right OpenGL cameras. -MJAPI mjtNum mjv_frustumHeight(const mjvScene* scn); +// Get frustum height at unit distance from camera; average left and right +// OpenGL cameras. +MJAPI mjtNum mjv_frustumHeight(const mjvScene *scn); -// Rotate 3D vec in horizontal plane by angle between (0,1) and (forward_x,forward_y). -MJAPI void mjv_alignToCamera(mjtNum res[3], const mjtNum vec[3], const mjtNum forward[3]); +// Rotate 3D vec in horizontal plane by angle between (0,1) and +// (forward_x,forward_y). +MJAPI void mjv_alignToCamera(mjtNum res[3], const mjtNum vec[3], + const mjtNum forward[3]); // Move camera with mouse; action is mjtMouse. -MJAPI void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, - const mjvScene* scn, mjvCamera* cam); +MJAPI void mjv_moveCamera(const mjModel *m, int action, mjtNum reldx, + mjtNum reldy, const mjvScene *scn, mjvCamera *cam); // Move camera with mouse given a scene state; action is mjtMouse. -MJAPI void mjv_moveCameraFromState(const mjvSceneState* scnstate, int action, +MJAPI void mjv_moveCameraFromState(const mjvSceneState *scnstate, int action, mjtNum reldx, mjtNum reldy, - const mjvScene* scn, mjvCamera* cam); + const mjvScene *scn, mjvCamera *cam); // Move perturb object with mouse; action is mjtMouse. -MJAPI void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx, - mjtNum reldy, const mjvScene* scn, mjvPerturb* pert); +MJAPI void mjv_movePerturb(const mjModel *m, const mjData *d, int action, + mjtNum reldx, mjtNum reldy, const mjvScene *scn, + mjvPerturb *pert); // Move perturb object with mouse given a scene state; action is mjtMouse. -MJAPI void mjv_movePerturbFromState(const mjvSceneState* scnstate, int action, +MJAPI void mjv_movePerturbFromState(const mjvSceneState *scnstate, int action, mjtNum reldx, mjtNum reldy, - const mjvScene* scn, mjvPerturb* pert); + const mjvScene *scn, mjvPerturb *pert); // Move model with mouse; action is mjtMouse. -MJAPI void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, - const mjtNum roomup[3], mjvScene* scn); +MJAPI void mjv_moveModel(const mjModel *m, int action, mjtNum reldx, + mjtNum reldy, const mjtNum roomup[3], mjvScene *scn); // Copy perturb pos,quat from selected body; set scale for perturbation. -MJAPI void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert); +MJAPI void mjv_initPerturb(const mjModel *m, mjData *d, const mjvScene *scn, + mjvPerturb *pert); -// Set perturb pos,quat in d->mocap when selected body is mocap, and in d->qpos otherwise. -// Write d->qpos only if flg_paused and subtree root for selected body has free joint. -MJAPI void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert, - int flg_paused); +// Set perturb pos,quat in d->mocap when selected body is mocap, and in d->qpos +// otherwise. Write d->qpos only if flg_paused and subtree root for selected +// body has free joint. +MJAPI void mjv_applyPerturbPose(const mjModel *m, mjData *d, + const mjvPerturb *pert, int flg_paused); // Set perturb force,torque in d->xfrc_applied, if selected body is dynamic. -MJAPI void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert); +MJAPI void mjv_applyPerturbForce(const mjModel *m, mjData *d, + const mjvPerturb *pert); // Return the average of two OpenGL cameras. -MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2); +MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera *cam1, + const mjvGLCamera *cam2); // Select geom, flex or skin with mouse, return bodyid; -1: none selected. -MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, +MJAPI int mjv_select(const mjModel *m, const mjData *d, const mjvOption *vopt, mjtNum aspectratio, mjtNum relx, mjtNum rely, - const mjvScene* scn, mjtNum selpnt[3], - int geomid[1], int flexid[1], int skinid[1]); + const mjvScene *scn, mjtNum selpnt[3], int geomid[1], + int flexid[1], int skinid[1]); - -//---------------------------------- Visualization ------------------------------------------------- +//---------------------------------- Visualization +//------------------------------------------------- // Set default visualization options. -MJAPI void mjv_defaultOption(mjvOption* opt); +MJAPI void mjv_defaultOption(mjvOption *opt); // Set default figure. -MJAPI void mjv_defaultFigure(mjvFigure* fig); +MJAPI void mjv_defaultFigure(mjvFigure *fig); -// Initialize given geom fields when not NULL, set the rest to their default values. -MJAPI void mjv_initGeom(mjvGeom* geom, int type, const mjtNum size[3], - const mjtNum pos[3], const mjtNum mat[9], const float rgba[4]); +// Initialize given geom fields when not NULL, set the rest to their default +// values. +MJAPI void mjv_initGeom(mjvGeom *geom, int type, const mjtNum size[3], + const mjtNum pos[3], const mjtNum mat[9], + const float rgba[4]); // Set (type, size, pos, mat) for connector-type geom between given points. // Assume that mjv_initGeom was already called to set all other properties. // Width of mjGEOM_LINE is denominated in pixels. // Deprecated: use mjv_connector. -MJAPI void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, - mjtNum a0, mjtNum a1, mjtNum a2, - mjtNum b0, mjtNum b1, mjtNum b2); +MJAPI void mjv_makeConnector(mjvGeom *geom, int type, mjtNum width, mjtNum a0, + mjtNum a1, mjtNum a2, mjtNum b0, mjtNum b1, + mjtNum b2); // Set (type, size, pos, mat) for connector-type geom between given points. // Assume that mjv_initGeom was already called to set all other properties. // Width of mjGEOM_LINE is denominated in pixels. -MJAPI void mjv_connector(mjvGeom* geom, int type, mjtNum width, +MJAPI void mjv_connector(mjvGeom *geom, int type, mjtNum width, const mjtNum from[3], const mjtNum to[3]); // Set default abstract scene. -MJAPI void mjv_defaultScene(mjvScene* scn); +MJAPI void mjv_defaultScene(mjvScene *scn); // Allocate resources in abstract scene. -MJAPI void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom); +MJAPI void mjv_makeScene(const mjModel *m, mjvScene *scn, int maxgeom); // Free abstract scene. -MJAPI void mjv_freeScene(mjvScene* scn); +MJAPI void mjv_freeScene(mjvScene *scn); // Update entire scene given model state. -MJAPI void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, - const mjvPerturb* pert, mjvCamera* cam, int catmask, mjvScene* scn); +MJAPI void mjv_updateScene(const mjModel *m, mjData *d, const mjvOption *opt, + const mjvPerturb *pert, mjvCamera *cam, int catmask, + mjvScene *scn); -// Update entire scene from a scene state, return the number of new mjWARN_VGEOMFULL warnings. -MJAPI int mjv_updateSceneFromState(const mjvSceneState* scnstate, const mjvOption* opt, - const mjvPerturb* pert, mjvCamera* cam, int catmask, - mjvScene* scn); +// Update entire scene from a scene state, return the number of new +// mjWARN_VGEOMFULL warnings. +MJAPI int mjv_updateSceneFromState(const mjvSceneState *scnstate, + const mjvOption *opt, const mjvPerturb *pert, + mjvCamera *cam, int catmask, mjvScene *scn); // Set default scene state. -MJAPI void mjv_defaultSceneState(mjvSceneState* scnstate); +MJAPI void mjv_defaultSceneState(mjvSceneState *scnstate); // Allocate resources and initialize a scene state object. -MJAPI void mjv_makeSceneState(const mjModel* m, const mjData* d, - mjvSceneState* scnstate, int maxgeom); +MJAPI void mjv_makeSceneState(const mjModel *m, const mjData *d, + mjvSceneState *scnstate, int maxgeom); // Free scene state. -MJAPI void mjv_freeSceneState(mjvSceneState* scnstate); +MJAPI void mjv_freeSceneState(mjvSceneState *scnstate); // Update a scene state from model and data. -MJAPI void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt, - mjvSceneState* scnstate); +MJAPI void mjv_updateSceneState(const mjModel *m, mjData *d, + const mjvOption *opt, mjvSceneState *scnstate); // Add geoms from selected categories. -MJAPI void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* opt, - const mjvPerturb* pert, int catmask, mjvScene* scn); +MJAPI void mjv_addGeoms(const mjModel *m, mjData *d, const mjvOption *opt, + const mjvPerturb *pert, int catmask, mjvScene *scn); // Make list of lights. -MJAPI void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn); +MJAPI void mjv_makeLights(const mjModel *m, mjData *d, mjvScene *scn); // Update camera. -MJAPI void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn); +MJAPI void mjv_updateCamera(const mjModel *m, mjData *d, mjvCamera *cam, + mjvScene *scn); // Update skins. -MJAPI void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn); - +MJAPI void mjv_updateSkin(const mjModel *m, mjData *d, mjvScene *scn); -//---------------------------------- OpenGL rendering ---------------------------------------------- +//---------------------------------- OpenGL rendering +//---------------------------------------------- // Set default mjrContext. -MJAPI void mjr_defaultContext(mjrContext* con); +MJAPI void mjr_defaultContext(mjrContext *con); // Allocate resources in custom OpenGL context; fontscale is mjtFontScale. -MJAPI void mjr_makeContext(const mjModel* m, mjrContext* con, int fontscale); +MJAPI void mjr_makeContext(const mjModel *m, mjrContext *con, int fontscale); // Change font of existing context. -MJAPI void mjr_changeFont(int fontscale, mjrContext* con); +MJAPI void mjr_changeFont(int fontscale, mjrContext *con); // Add Aux buffer with given index to context; free previous Aux buffer. -MJAPI void mjr_addAux(int index, int width, int height, int samples, mjrContext* con); +MJAPI void mjr_addAux(int index, int width, int height, int samples, + mjrContext *con); // Free resources in custom OpenGL context, set to default. -MJAPI void mjr_freeContext(mjrContext* con); +MJAPI void mjr_freeContext(mjrContext *con); // Resize offscreen buffers. -MJAPI void mjr_resizeOffscreen(int width, int height, mjrContext* con); +MJAPI void mjr_resizeOffscreen(int width, int height, mjrContext *con); // Upload texture to GPU, overwriting previous upload if any. -MJAPI void mjr_uploadTexture(const mjModel* m, const mjrContext* con, int texid); +MJAPI void mjr_uploadTexture(const mjModel *m, const mjrContext *con, + int texid); // Upload mesh to GPU, overwriting previous upload if any. -MJAPI void mjr_uploadMesh(const mjModel* m, const mjrContext* con, int meshid); +MJAPI void mjr_uploadMesh(const mjModel *m, const mjrContext *con, int meshid); // Upload height field to GPU, overwriting previous upload if any. -MJAPI void mjr_uploadHField(const mjModel* m, const mjrContext* con, int hfieldid); +MJAPI void mjr_uploadHField(const mjModel *m, const mjrContext *con, + int hfieldid); // Make con->currentBuffer current again. -MJAPI void mjr_restoreBuffer(const mjrContext* con); +MJAPI void mjr_restoreBuffer(const mjrContext *con); // Set OpenGL framebuffer for rendering: mjFB_WINDOW or mjFB_OFFSCREEN. -// If only one buffer is available, set that buffer and ignore framebuffer argument. -MJAPI void mjr_setBuffer(int framebuffer, mjrContext* con); +// If only one buffer is available, set that buffer and ignore framebuffer +// argument. +MJAPI void mjr_setBuffer(int framebuffer, mjrContext *con); // Read pixels from current OpenGL framebuffer to client buffer. // Viewport is in OpenGL framebuffer; client buffer starts at (0,0). -MJAPI void mjr_readPixels(unsigned char* rgb, float* depth, - mjrRect viewport, const mjrContext* con); +MJAPI void mjr_readPixels(unsigned char *rgb, float *depth, mjrRect viewport, + const mjrContext *con); // Draw pixels from client buffer to current OpenGL framebuffer. // Viewport is in OpenGL framebuffer; client buffer starts at (0,0). -MJAPI void mjr_drawPixels(const unsigned char* rgb, const float* depth, - mjrRect viewport, const mjrContext* con); +MJAPI void mjr_drawPixels(const unsigned char *rgb, const float *depth, + mjrRect viewport, const mjrContext *con); -// Blit from src viewpoint in current framebuffer to dst viewport in other framebuffer. -// If src, dst have different size and flg_depth==0, color is interpolated with GL_LINEAR. -MJAPI void mjr_blitBuffer(mjrRect src, mjrRect dst, - int flg_color, int flg_depth, const mjrContext* con); +// Blit from src viewpoint in current framebuffer to dst viewport in other +// framebuffer. If src, dst have different size and flg_depth==0, color is +// interpolated with GL_LINEAR. +MJAPI void mjr_blitBuffer(mjrRect src, mjrRect dst, int flg_color, + int flg_depth, const mjrContext *con); // Set Aux buffer for custom OpenGL rendering (call restoreBuffer when done). -MJAPI void mjr_setAux(int index, const mjrContext* con); +MJAPI void mjr_setAux(int index, const mjrContext *con); // Blit from Aux buffer to con->currentBuffer. -MJAPI void mjr_blitAux(int index, mjrRect src, int left, int bottom, const mjrContext* con); +MJAPI void mjr_blitAux(int index, mjrRect src, int left, int bottom, + const mjrContext *con); // Draw text at (x,y) in relative coordinates; font is mjtFont. -MJAPI void mjr_text(int font, const char* txt, const mjrContext* con, - float x, float y, float r, float g, float b); +MJAPI void mjr_text(int font, const char *txt, const mjrContext *con, float x, + float y, float r, float g, float b); // Draw text overlay; font is mjtFont; gridpos is mjtGridPos. MJAPI void mjr_overlay(int font, int gridpos, mjrRect viewport, - const char* overlay, const char* overlay2, const mjrContext* con); + const char *overlay, const char *overlay2, + const mjrContext *con); // Get maximum viewport for active buffer. -MJAPI mjrRect mjr_maxViewport(const mjrContext* con); +MJAPI mjrRect mjr_maxViewport(const mjrContext *con); // Draw rectangle. MJAPI void mjr_rectangle(mjrRect viewport, float r, float g, float b, float a); // Draw rectangle with centered text. -MJAPI void mjr_label(mjrRect viewport, int font, const char* txt, - float r, float g, float b, float a, float rt, float gt, float bt, - const mjrContext* con); +MJAPI void mjr_label(mjrRect viewport, int font, const char *txt, float r, + float g, float b, float a, float rt, float gt, float bt, + const mjrContext *con); // Draw 2D figure. -MJAPI void mjr_figure(mjrRect viewport, mjvFigure* fig, const mjrContext* con); +MJAPI void mjr_figure(mjrRect viewport, mjvFigure *fig, const mjrContext *con); // Render 3D scene. -MJAPI void mjr_render(mjrRect viewport, mjvScene* scn, const mjrContext* con); +MJAPI void mjr_render(mjrRect viewport, mjvScene *scn, const mjrContext *con); // Call glFinish. MJAPI void mjr_finish(void); @@ -782,10 +856,10 @@ MJAPI void mjr_finish(void); MJAPI int mjr_getError(void); // Find first rectangle containing mouse, -1: not found. -MJAPI int mjr_findRect(int x, int y, int nrect, const mjrRect* rect); - +MJAPI int mjr_findRect(int x, int y, int nrect, const mjrRect *rect); -//---------------------------------- UI framework -------------------------------------------------- +//---------------------------------- UI framework +//-------------------------------------------------- // Get builtin UI theme spacing (ind: 0-1). MJAPI mjuiThemeSpacing mjui_themeSpacing(int ind); @@ -794,100 +868,100 @@ MJAPI mjuiThemeSpacing mjui_themeSpacing(int ind); MJAPI mjuiThemeColor mjui_themeColor(int ind); // Add definitions to UI. -MJAPI void mjui_add(mjUI* ui, const mjuiDef* def); +MJAPI void mjui_add(mjUI *ui, const mjuiDef *def); // Add definitions to UI section. -MJAPI void mjui_addToSection(mjUI* ui, int sect, const mjuiDef* def); +MJAPI void mjui_addToSection(mjUI *ui, int sect, const mjuiDef *def); // Compute UI sizes. -MJAPI void mjui_resize(mjUI* ui, const mjrContext* con); +MJAPI void mjui_resize(mjUI *ui, const mjrContext *con); // Update specific section/item; -1: update all. -MJAPI void mjui_update(int section, int item, const mjUI* ui, - const mjuiState* state, const mjrContext* con); +MJAPI void mjui_update(int section, int item, const mjUI *ui, + const mjuiState *state, const mjrContext *con); // Handle UI event, return pointer to changed item, NULL if no change. -MJAPI mjuiItem* mjui_event(mjUI* ui, mjuiState* state, const mjrContext* con); +MJAPI mjuiItem *mjui_event(mjUI *ui, mjuiState *state, const mjrContext *con); // Copy UI image to current buffer. -MJAPI void mjui_render(mjUI* ui, const mjuiState* state, const mjrContext* con); +MJAPI void mjui_render(mjUI *ui, const mjuiState *state, const mjrContext *con); - -//---------------------------------- Error and memory ---------------------------------------------- +//---------------------------------- Error and memory +//---------------------------------------------- // Main error function; does not return to caller. -MJAPI void mju_error(const char* msg, ...) mjPRINTFLIKE(1, 2); +MJAPI void mju_error(const char *msg, ...) mjPRINTFLIKE(1, 2); // Deprecated: use mju_error. -MJAPI void mju_error_i(const char* msg, int i); +MJAPI void mju_error_i(const char *msg, int i); // Deprecated: use mju_error. -MJAPI void mju_error_s(const char* msg, const char* text); +MJAPI void mju_error_s(const char *msg, const char *text); // Main warning function; returns to caller. -MJAPI void mju_warning(const char* msg, ...) mjPRINTFLIKE(1, 2); +MJAPI void mju_warning(const char *msg, ...) mjPRINTFLIKE(1, 2); // Deprecated: use mju_warning. -MJAPI void mju_warning_i(const char* msg, int i); +MJAPI void mju_warning_i(const char *msg, int i); // Deprecated: use mju_warning. -MJAPI void mju_warning_s(const char* msg, const char* text); +MJAPI void mju_warning_s(const char *msg, const char *text); // Clear user error and memory handlers. MJAPI void mju_clearHandlers(void); // Allocate memory; byte-align on 64; pad size to multiple of 64. -MJAPI void* mju_malloc(size_t size); +MJAPI void *mju_malloc(size_t size); // Free memory, using free() by default. -MJAPI void mju_free(void* ptr); +MJAPI void mju_free(void *ptr); // High-level warning function: count warnings in mjData, print only the first. -MJAPI void mj_warning(mjData* d, int warning, int info); +MJAPI void mj_warning(mjData *d, int warning, int info); // Write [datetime, type: message] to MUJOCO_LOG.TXT. -MJAPI void mju_writeLog(const char* type, const char* msg); - +MJAPI void mju_writeLog(const char *type, const char *msg); -//---------------------------------- Standard math ------------------------------------------------- +//---------------------------------- Standard math +//------------------------------------------------- #ifdef mjUSEDOUBLE - #define mju_sqrt sqrt - #define mju_exp exp - #define mju_sin sin - #define mju_cos cos - #define mju_tan tan - #define mju_asin asin - #define mju_acos acos - #define mju_atan2 atan2 - #define mju_tanh tanh - #define mju_pow pow - #define mju_abs fabs - #define mju_log log - #define mju_log10 log10 - #define mju_floor floor - #define mju_ceil ceil +#define mju_sqrt sqrt +#define mju_exp exp +#define mju_sin sin +#define mju_cos cos +#define mju_tan tan +#define mju_asin asin +#define mju_acos acos +#define mju_atan2 atan2 +#define mju_tanh tanh +#define mju_pow pow +#define mju_abs fabs +#define mju_log log +#define mju_log10 log10 +#define mju_floor floor +#define mju_ceil ceil #else - #define mju_sqrt sqrtf - #define mju_exp expf - #define mju_sin sinf - #define mju_cos cosf - #define mju_tan tanf - #define mju_asin asinf - #define mju_acos acosf - #define mju_atan2 atan2f - #define mju_tanh tanhf - #define mju_pow powf - #define mju_abs fabsf - #define mju_log logf - #define mju_log10 log10f - #define mju_floor floorf - #define mju_ceil ceilf +#define mju_sqrt sqrtf +#define mju_exp expf +#define mju_sin sinf +#define mju_cos cosf +#define mju_tan tanf +#define mju_asin asinf +#define mju_acos acosf +#define mju_atan2 atan2f +#define mju_tanh tanhf +#define mju_pow powf +#define mju_abs fabsf +#define mju_log logf +#define mju_log10 log10f +#define mju_floor floorf +#define mju_ceil ceilf #endif - -//---------------------------------- Vector math --------------------------------------------------- +//---------------------------------- Vector math +//--------------------------------------------------- // Set res = 0. MJAPI void mju_zero3(mjtNum res[3]); @@ -914,7 +988,8 @@ MJAPI void mju_subFrom3(mjtNum res[3], const mjtNum vec[3]); MJAPI void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl); // Set res = vec1 + vec2*scl. -MJAPI void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl); +MJAPI void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], + const mjtNum vec2[3], mjtNum scl); // Normalize vector, return length before normalization. MJAPI mjtNum mju_normalize3(mjtNum vec[3]); @@ -929,10 +1004,12 @@ MJAPI mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]); MJAPI mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]); // Multiply vector by 3D rotation matrix: res = mat * vec. -MJAPI void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); +MJAPI void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], + const mjtNum mat[9]); // Multiply vector by transposed 3D rotation matrix: res = mat' * vec. -MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); +MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], + const mjtNum mat[9]); // Compute cross-product: res = cross(a, b). MJAPI void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]); @@ -950,105 +1027,119 @@ MJAPI void mju_copy4(mjtNum res[4], const mjtNum data[4]); MJAPI mjtNum mju_normalize4(mjtNum vec[4]); // Set res = 0. -MJAPI void mju_zero(mjtNum* res, int n); +MJAPI void mju_zero(mjtNum *res, int n); // Set res = val. -MJAPI void mju_fill(mjtNum* res, mjtNum val, int n); +MJAPI void mju_fill(mjtNum *res, mjtNum val, int n); // Set res = vec. -MJAPI void mju_copy(mjtNum* res, const mjtNum* vec, int n); +MJAPI void mju_copy(mjtNum *res, const mjtNum *vec, int n); // Return sum(vec). -MJAPI mjtNum mju_sum(const mjtNum* vec, int n); +MJAPI mjtNum mju_sum(const mjtNum *vec, int n); // Return L1 norm: sum(abs(vec)). -MJAPI mjtNum mju_L1(const mjtNum* vec, int n); +MJAPI mjtNum mju_L1(const mjtNum *vec, int n); // Set res = vec*scl. -MJAPI void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n); +MJAPI void mju_scl(mjtNum *res, const mjtNum *vec, mjtNum scl, int n); // Set res = vec1 + vec2. -MJAPI void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n); +MJAPI void mju_add(mjtNum *res, const mjtNum *vec1, const mjtNum *vec2, int n); // Set res = vec1 - vec2. -MJAPI void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n); +MJAPI void mju_sub(mjtNum *res, const mjtNum *vec1, const mjtNum *vec2, int n); // Set res = res + vec. -MJAPI void mju_addTo(mjtNum* res, const mjtNum* vec, int n); +MJAPI void mju_addTo(mjtNum *res, const mjtNum *vec, int n); // Set res = res - vec. -MJAPI void mju_subFrom(mjtNum* res, const mjtNum* vec, int n); +MJAPI void mju_subFrom(mjtNum *res, const mjtNum *vec, int n); // Set res = res + vec*scl. -MJAPI void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n); +MJAPI void mju_addToScl(mjtNum *res, const mjtNum *vec, mjtNum scl, int n); // Set res = vec1 + vec2*scl. -MJAPI void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, int n); +MJAPI void mju_addScl(mjtNum *res, const mjtNum *vec1, const mjtNum *vec2, + mjtNum scl, int n); // Normalize vector, return length before normalization. -MJAPI mjtNum mju_normalize(mjtNum* res, int n); +MJAPI mjtNum mju_normalize(mjtNum *res, int n); // Return vector length (without normalizing vector). -MJAPI mjtNum mju_norm(const mjtNum* res, int n); +MJAPI mjtNum mju_norm(const mjtNum *res, int n); // Return dot-product of vec1 and vec2. -MJAPI mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n); +MJAPI mjtNum mju_dot(const mjtNum *vec1, const mjtNum *vec2, int n); // Multiply matrix and vector: res = mat * vec. -MJAPI void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc); +MJAPI void mju_mulMatVec(mjtNum *res, const mjtNum *mat, const mjtNum *vec, + int nr, int nc); // Multiply transposed matrix and vector: res = mat' * vec. -MJAPI void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc); +MJAPI void mju_mulMatTVec(mjtNum *res, const mjtNum *mat, const mjtNum *vec, + int nr, int nc); -// Multiply square matrix with vectors on both sides: returns vec1' * mat * vec2. -MJAPI mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec2, int n); +// Multiply square matrix with vectors on both sides: returns vec1' * mat * +// vec2. +MJAPI mjtNum mju_mulVecMatVec(const mjtNum *vec1, const mjtNum *mat, + const mjtNum *vec2, int n); // Transpose matrix: res = mat'. -MJAPI void mju_transpose(mjtNum* res, const mjtNum* mat, int nr, int nc); +MJAPI void mju_transpose(mjtNum *res, const mjtNum *mat, int nr, int nc); // Symmetrize square matrix res = (mat + mat')/2. -MJAPI void mju_symmetrize(mjtNum* res, const mjtNum* mat, int n); +MJAPI void mju_symmetrize(mjtNum *res, const mjtNum *mat, int n); // Set mat to the identity matrix. -MJAPI void mju_eye(mjtNum* mat, int n); +MJAPI void mju_eye(mjtNum *mat, int n); // Multiply matrices: res = mat1 * mat2. -MJAPI void mju_mulMatMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2, +MJAPI void mju_mulMatMat(mjtNum *res, const mjtNum *mat1, const mjtNum *mat2, int r1, int c1, int c2); // Multiply matrices, second argument transposed: res = mat1 * mat2'. -MJAPI void mju_mulMatMatT(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2, +MJAPI void mju_mulMatMatT(mjtNum *res, const mjtNum *mat1, const mjtNum *mat2, int r1, int c1, int r2); // Multiply matrices, first argument transposed: res = mat1' * mat2. -MJAPI void mju_mulMatTMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2, +MJAPI void mju_mulMatTMat(mjtNum *res, const mjtNum *mat1, const mjtNum *mat2, int r1, int c1, int c2); -// Set res = mat' * diag * mat if diag is not NULL, and res = mat' * mat otherwise. -MJAPI void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, int nc); - -// Coordinate transform of 6D motion or force vector in rotation:translation format. -// rotnew2old is 3-by-3, NULL means no rotation; flg_force specifies force or motion type. -MJAPI void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force, - const mjtNum newpos[3], const mjtNum oldpos[3], +// Set res = mat' * diag * mat if diag is not NULL, and res = mat' * mat +// otherwise. +MJAPI void mju_sqrMatTD(mjtNum *res, const mjtNum *mat, const mjtNum *diag, + int nr, int nc); + +// Coordinate transform of 6D motion or force vector in rotation:translation +// format. rotnew2old is 3-by-3, NULL means no rotation; flg_force specifies +// force or motion type. +MJAPI void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], + int flg_force, const mjtNum newpos[3], + const mjtNum oldpos[3], const mjtNum rotnew2old[9]); -//---------------------------------- Quaternions --------------------------------------------------- +//---------------------------------- Quaternions +//--------------------------------------------------- // Rotate vector by quaternion. -MJAPI void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]); +MJAPI void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], + const mjtNum quat[4]); // Conjugate quaternion, corresponding to opposite rotation. MJAPI void mju_negQuat(mjtNum res[4], const mjtNum quat[4]); // Multiply quaternions. -MJAPI void mju_mulQuat(mjtNum res[4], const mjtNum quat1[4], const mjtNum quat2[4]); +MJAPI void mju_mulQuat(mjtNum res[4], const mjtNum quat1[4], + const mjtNum quat2[4]); // Multiply quaternion and axis. -MJAPI void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3]); +MJAPI void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], + const mjtNum axis[3]); // Convert axisAngle to quaternion. -MJAPI void mju_axisAngle2Quat(mjtNum res[4], const mjtNum axis[3], mjtNum angle); +MJAPI void mju_axisAngle2Quat(mjtNum res[4], const mjtNum axis[3], + mjtNum angle); // Convert quaternion (corresponding to orientation difference) to 3D velocity. MJAPI void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt); @@ -1063,7 +1154,8 @@ MJAPI void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]); MJAPI void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]); // Compute time-derivative of quaternion, given 3D rotational velocity. -MJAPI void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]); +MJAPI void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], + const mjtNum vel[3]); // Integrate quaternion given 3D angular velocity. MJAPI void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale); @@ -1072,10 +1164,13 @@ MJAPI void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale); MJAPI void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]); // Convert sequence of Euler angles (radians) to quaternion. -// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations. -MJAPI void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq); +// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic +// rotations. +MJAPI void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], + const char *seq); -//---------------------------------- Poses --------------------------------------------------------- +//---------------------------------- Poses +//--------------------------------------------------------- // Multiply two poses. MJAPI void mju_mulPose(mjtNum posres[3], mjtNum quatres[4], @@ -1083,56 +1178,64 @@ MJAPI void mju_mulPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos2[3], const mjtNum quat2[4]); // Conjugate pose, corresponding to the opposite spatial transformation. -MJAPI void mju_negPose(mjtNum posres[3], mjtNum quatres[4], - const mjtNum pos[3], const mjtNum quat[4]); +MJAPI void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], + const mjtNum quat[4]); // Transform vector by pose. -MJAPI void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], - const mjtNum vec[3]); +MJAPI void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], + const mjtNum quat[4], const mjtNum vec[3]); +//--------------------------------- Decompositions / Solvers +//--------------------------------------- -//--------------------------------- Decompositions / Solvers --------------------------------------- - -// Cholesky decomposition: mat = L*L'; return rank, decomposition performed in-place into mat. -MJAPI int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag); +// Cholesky decomposition: mat = L*L'; return rank, decomposition performed +// in-place into mat. +MJAPI int mju_cholFactor(mjtNum *mat, int n, mjtNum mindiag); // Solve (mat*mat') * res = vec, where mat is a Cholesky factor. -MJAPI void mju_cholSolve(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n); +MJAPI void mju_cholSolve(mjtNum *res, const mjtNum *mat, const mjtNum *vec, + int n); // Cholesky rank-one update: L*L' +/- x*x'; return rank. -MJAPI int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus); +MJAPI int mju_cholUpdate(mjtNum *mat, mjtNum *x, int n, int flg_plus); // Band-dense Cholesky decomposition. // Returns minimum value in the factorized diagonal, or 0 if rank-deficient. // mat has (ntotal-ndense) x nband + ndense x ntotal elements. -// The first (ntotal-ndense) x nband store the band part, left of diagonal, inclusive. -// The second ndense x ntotal store the band part as entire dense rows. -// Add diagadd+diagmul*mat_ii to diagonal before factorization. -MJAPI mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense, +// The first (ntotal-ndense) x nband store the band part, left of diagonal, +// inclusive. The second ndense x ntotal store the band part as entire dense +// rows. Add diagadd+diagmul*mat_ii to diagonal before factorization. +MJAPI mjtNum mju_cholFactorBand(mjtNum *mat, int ntotal, int nband, int ndense, mjtNum diagadd, mjtNum diagmul); // Solve (mat*mat')*res = vec where mat is a band-dense Cholesky factor. -MJAPI void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec, +MJAPI void mju_cholSolveBand(mjtNum *res, const mjtNum *mat, const mjtNum *vec, int ntotal, int nband, int ndense); // Convert banded matrix to dense matrix, fill upper triangle if flg_sym>0. -MJAPI void mju_band2Dense(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int ndense, - mjtByte flg_sym); +MJAPI void mju_band2Dense(mjtNum *res, const mjtNum *mat, int ntotal, int nband, + int ndense, mjtByte flg_sym); // Convert dense matrix to banded matrix. -MJAPI void mju_dense2Band(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int ndense); +MJAPI void mju_dense2Band(mjtNum *res, const mjtNum *mat, int ntotal, int nband, + int ndense); -// Multiply band-diagonal matrix with nvec vectors, include upper triangle if flg_sym>0. -MJAPI void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, - int ntotal, int nband, int ndense, int nvec, mjtByte flg_sym); +// Multiply band-diagonal matrix with nvec vectors, include upper triangle if +// flg_sym>0. +MJAPI void mju_bandMulMatVec(mjtNum *res, const mjtNum *mat, const mjtNum *vec, + int ntotal, int nband, int ndense, int nvec, + mjtByte flg_sym); // Address of diagonal element i in band-dense matrix representation. MJAPI int mju_bandDiag(int i, int ntotal, int nband, int ndense); -// Eigenvalue decomposition of symmetric 3x3 matrix, mat = eigvec * diag(eigval) * eigvec'. -MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]); +// Eigenvalue decomposition of symmetric 3x3 matrix, mat = eigvec * diag(eigval) +// * eigvec'. +MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], + const mjtNum mat[9]); -// minimize 0.5*x'*H*x + x'*g s.t. lower <= x <= upper, return rank or -1 if failed +// minimize 0.5*x'*H*x + x'*g s.t. lower <= x <= upper, return rank or -1 if +// failed // inputs: // n - problem dimension // H - SPD matrix n*n @@ -1149,26 +1252,31 @@ MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjt // index - set of free dimensions nfree allocated: n // notes: // the initial value of res is used to warmstart the solver -// R must have allocatd size n*(n+7), but only nfree*nfree values are used in output -// index (if given) must have allocated size n, but only nfree values are used in output -// only the lower triangles of H and R and are read from and written to, respectively -// the convenience function mju_boxQPmalloc allocates the required data structures -MJAPI int mju_boxQP(mjtNum* res, mjtNum* R, int* index, const mjtNum* H, const mjtNum* g, int n, - const mjtNum* lower, const mjtNum* upper); +// R must have allocatd size n*(n+7), but only nfree*nfree values are used +// in output index (if given) must have allocated size n, but only nfree +// values are used in output only the lower triangles of H and R and are +// read from and written to, respectively the convenience function +// mju_boxQPmalloc allocates the required data structures +MJAPI int mju_boxQP(mjtNum *res, mjtNum *R, int *index, const mjtNum *H, + const mjtNum *g, int n, const mjtNum *lower, + const mjtNum *upper); // allocate heap memory for box-constrained Quadratic Program // as in mju_boxQP, index, lower, and upper are optional // free all pointers with mju_free() -MJAPI void mju_boxQPmalloc(mjtNum** res, mjtNum** R, int** index, mjtNum** H, mjtNum** g, int n, - mjtNum** lower, mjtNum** upper); +MJAPI void mju_boxQPmalloc(mjtNum **res, mjtNum **R, int **index, mjtNum **H, + mjtNum **g, int n, mjtNum **lower, mjtNum **upper); -//---------------------- Miscellaneous ------------------------------------------------------------- +//---------------------- Miscellaneous +//------------------------------------------------------------- -// Muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax). +// Muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, +// fvmax). MJAPI mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2], mjtNum acc0, const mjtNum prm[9]); -// Muscle passive force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax). +// Muscle passive force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, +// fvmax). MJAPI mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2], mjtNum acc0, const mjtNum prm[9]); @@ -1176,13 +1284,16 @@ MJAPI mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2], MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]); // Convert contact force to pyramid representation. -MJAPI void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, int dim); +MJAPI void mju_encodePyramid(mjtNum *pyramid, const mjtNum *force, + const mjtNum *mu, int dim); // Convert pyramid representation to contact force. -MJAPI void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, const mjtNum* mu, int dim); +MJAPI void mju_decodePyramid(mjtNum *force, const mjtNum *pyramid, + const mjtNum *mu, int dim); // Integrate spring-damper analytically, return pos(dt). -MJAPI mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, mjtNum dt); +MJAPI mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, + mjtNum dt); // Return min(a,b) with single evaluation of a and b. MJAPI mjtNum mju_min(mjtNum a, mjtNum b); @@ -1200,55 +1311,55 @@ MJAPI mjtNum mju_sign(mjtNum x); MJAPI int mju_round(mjtNum x); // Convert type id (mjtObj) to type name. -MJAPI const char* mju_type2Str(int type); +MJAPI const char *mju_type2Str(int type); // Convert type name to type id (mjtObj). -MJAPI int mju_str2Type(const char* str); +MJAPI int mju_str2Type(const char *str); // Return human readable number of bytes using standard letter suffix. -MJAPI const char* mju_writeNumBytes(size_t nbytes); +MJAPI const char *mju_writeNumBytes(size_t nbytes); // Construct a warning message given the warning type and info. -MJAPI const char* mju_warningText(int warning, size_t info); +MJAPI const char *mju_warningText(int warning, size_t info); // Return 1 if nan or abs(x)>mjMAXVAL, 0 otherwise. Used by check functions. MJAPI int mju_isBad(mjtNum x); // Return 1 if all elements are 0. -MJAPI int mju_isZero(mjtNum* vec, int n); +MJAPI int mju_isZero(mjtNum *vec, int n); // Standard normal random number generator (optional second number). -MJAPI mjtNum mju_standardNormal(mjtNum* num2); +MJAPI mjtNum mju_standardNormal(mjtNum *num2); // Convert from float to mjtNum. -MJAPI void mju_f2n(mjtNum* res, const float* vec, int n); +MJAPI void mju_f2n(mjtNum *res, const float *vec, int n); // Convert from mjtNum to float. -MJAPI void mju_n2f(float* res, const mjtNum* vec, int n); +MJAPI void mju_n2f(float *res, const mjtNum *vec, int n); // Convert from double to mjtNum. -MJAPI void mju_d2n(mjtNum* res, const double* vec, int n); +MJAPI void mju_d2n(mjtNum *res, const double *vec, int n); // Convert from mjtNum to double. -MJAPI void mju_n2d(double* res, const mjtNum* vec, int n); +MJAPI void mju_n2d(double *res, const mjtNum *vec, int n); // Insertion sort, resulting list is in increasing order. -MJAPI void mju_insertionSort(mjtNum* list, int n); +MJAPI void mju_insertionSort(mjtNum *list, int n); // Integer insertion sort, resulting list is in increasing order. -MJAPI void mju_insertionSortInt(int* list, int n); +MJAPI void mju_insertionSortInt(int *list, int n); // Generate Halton sequence. MJAPI mjtNum mju_Halton(int index, int base); // Call strncpy, then set dst[n-1] = 0. -MJAPI char* mju_strncpy(char *dst, const char *src, int n); +MJAPI char *mju_strncpy(char *dst, const char *src, int n); // Sigmoid function over 0<=x<=1 using quintic polynomial. MJAPI mjtNum mju_sigmoid(mjtNum x); - -//---------------------- Derivatives --------------------------------------------------------------- +//---------------------- Derivatives +//--------------------------------------------------------------- // Finite differenced transition matrices (control theory notation) // d(x_next) = A*dx + B*du @@ -1258,11 +1369,14 @@ MJAPI mjtNum mju_sigmoid(mjtNum x); // B: (2*nv+na x nu) // D: (nsensordata x 2*nv+na) // C: (nsensordata x nu) -MJAPI void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, - mjtNum* A, mjtNum* B, mjtNum* C, mjtNum* D); - -// Finite differenced Jacobians of (force, sensors) = mj_inverse(state, acceleration) -// All outputs are optional. Output dimensions (transposed w.r.t Control Theory convention): +MJAPI void mjd_transitionFD(const mjModel *m, mjData *d, mjtNum eps, + mjtByte flg_centered, mjtNum *A, mjtNum *B, + mjtNum *C, mjtNum *D); + +// Finite differenced Jacobians of (force, sensors) = mj_inverse(state, +// acceleration) +// All outputs are optional. Output dimensions (transposed w.r.t Control +// Theory convention): // DfDq: (nv x nv) // DfDv: (nv x nv) // DfDa: (nv x nv) @@ -1275,111 +1389,122 @@ MJAPI void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg // outputs: f=qfrc_inverse, s=sensordata, m=qM // notes: // optionally computes mass matrix Jacobian DmDq -// flg_actuation specifies whether to subtract qfrc_actuator from qfrc_inverse -MJAPI void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuation, - mjtNum *DfDq, mjtNum *DfDv, mjtNum *DfDa, - mjtNum *DsDq, mjtNum *DsDv, mjtNum *DsDa, +// flg_actuation specifies whether to subtract qfrc_actuator from +// qfrc_inverse +MJAPI void mjd_inverseFD(const mjModel *m, mjData *d, mjtNum eps, + mjtByte flg_actuation, mjtNum *DfDq, mjtNum *DfDv, + mjtNum *DfDa, mjtNum *DsDq, mjtNum *DsDv, mjtNum *DsDa, mjtNum *DmDq); // Derivatives of mju_subQuat. -MJAPI void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db[9]); +MJAPI void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], + mjtNum Db[9]); // Derivatives of mju_quatIntegrate. -MJAPI void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale, - mjtNum Dquat[9], mjtNum Dvel[9], mjtNum Dscale[3]); +MJAPI void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale, mjtNum Dquat[9], + mjtNum Dvel[9], mjtNum Dscale[3]); -//---------------------- Plugins ------------------------------------------------------------------- +//---------------------- Plugins +//------------------------------------------------------------------- // Set default plugin definition. -MJAPI void mjp_defaultPlugin(mjpPlugin* plugin); +MJAPI void mjp_defaultPlugin(mjpPlugin *plugin); // Globally register a plugin. This function is thread-safe. // If an identical mjpPlugin is already registered, this function does nothing. -// If a non-identical mjpPlugin with the same name is already registered, an mju_error is raised. -// Two mjpPlugins are considered identical if all member function pointers and numbers are equal, -// and the name and attribute strings are all identical, however the char pointers to the strings -// need not be the same. -MJAPI int mjp_registerPlugin(const mjpPlugin* plugin); +// If a non-identical mjpPlugin with the same name is already registered, an +// mju_error is raised. Two mjpPlugins are considered identical if all member +// function pointers and numbers are equal, and the name and attribute strings +// are all identical, however the char pointers to the strings need not be the +// same. +MJAPI int mjp_registerPlugin(const mjpPlugin *plugin); // Return the number of globally registered plugins. MJAPI int mjp_pluginCount(void); -// Look up a plugin by name. If slot is not NULL, also write its registered slot number into it. -MJAPI const mjpPlugin* mjp_getPlugin(const char* name, int* slot); +// Look up a plugin by name. If slot is not NULL, also write its registered slot +// number into it. +MJAPI const mjpPlugin *mjp_getPlugin(const char *name, int *slot); -// Look up a plugin by the registered slot number that was returned by mjp_registerPlugin. -MJAPI const mjpPlugin* mjp_getPluginAtSlot(int slot); +// Look up a plugin by the registered slot number that was returned by +// mjp_registerPlugin. +MJAPI const mjpPlugin *mjp_getPluginAtSlot(int slot); // Set default resource provider definition. -MJAPI void mjp_defaultResourceProvider(mjpResourceProvider* provider); +MJAPI void mjp_defaultResourceProvider(mjpResourceProvider *provider); -// Globally register a resource provider in a thread-safe manner. The provider must have a prefix -// that is not a sub-prefix or super-prefix of any current registered providers. This function -// returns a slot number > 0 on success. -MJAPI int mjp_registerResourceProvider(const mjpResourceProvider* provider); +// Globally register a resource provider in a thread-safe manner. The provider +// must have a prefix that is not a sub-prefix or super-prefix of any current +// registered providers. This function returns a slot number > 0 on success. +MJAPI int mjp_registerResourceProvider(const mjpResourceProvider *provider); // Return the number of globally registered resource providers. MJAPI int mjp_resourceProviderCount(void); -// Return the resource provider with the prefix that matches against the resource name. -// If no match, return NULL. -MJAPI const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name); +// Return the resource provider with the prefix that matches against the +// resource name. If no match, return NULL. +MJAPI const mjpResourceProvider * +mjp_getResourceProvider(const char *resource_name); -// Look up a resource provider by slot number returned by mjp_registerResourceProvider. -// If invalid slot number, return NULL. -MJAPI const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot); +// Look up a resource provider by slot number returned by +// mjp_registerResourceProvider. If invalid slot number, return NULL. +MJAPI const mjpResourceProvider *mjp_getResourceProviderAtSlot(int slot); -//---------------------- Thread ------------------------------------------------------------------- +//---------------------- Thread +//------------------------------------------------------------------- // Create a thread pool with the specified number of threads running. -MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads); +MJAPI mjThreadPool *mju_threadPoolCreate(size_t number_of_threads); // Adds a thread pool to mjData and configures it for multi-threaded use. -MJAPI void mju_bindThreadPool(mjData* d, void* thread_pool); +MJAPI void mju_bindThreadPool(mjData *d, void *thread_pool); // Enqueue a task in a thread pool. -MJAPI void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task); +MJAPI void mju_threadPoolEnqueue(mjThreadPool *thread_pool, mjTask *task); // Destroy a thread pool. -MJAPI void mju_threadPoolDestroy(mjThreadPool* thread_pool); +MJAPI void mju_threadPoolDestroy(mjThreadPool *thread_pool); // Initialize an mjTask. -MJAPI void mju_defaultTask(mjTask* task); +MJAPI void mju_defaultTask(mjTask *task); // Wait for a task to complete. -MJAPI void mju_taskJoin(mjTask* task); +MJAPI void mju_taskJoin(mjTask *task); -//---------------------- Sanitizer instrumentation helpers ----------------------------------------- +//---------------------- Sanitizer instrumentation helpers +//----------------------------------------- // -// Most MuJoCo users can ignore these functions, the following comments are aimed primarily at -// MuJoCo developers. +// Most MuJoCo users can ignore these functions, the following comments are +// aimed primarily at MuJoCo developers. // -// When built and run under address sanitizer (asan), mj_markStack and mj_freeStack are instrumented -// to detect leakage of mjData stack frames. When the compiler inlines several callees that call -// into mark/free into the same function, this instrumentation requires that the compiler retains -// separate mark/free calls for each original callee. The memory-clobbered asm blocks act as a -// barrier to prevent mark/free calls from being combined under optimization. +// When built and run under address sanitizer (asan), mj_markStack and +// mj_freeStack are instrumented to detect leakage of mjData stack frames. When +// the compiler inlines several callees that call into mark/free into the same +// function, this instrumentation requires that the compiler retains separate +// mark/free calls for each original callee. The memory-clobbered asm blocks act +// as a barrier to prevent mark/free calls from being combined under +// optimization. #ifdef ADDRESS_SANITIZER -void mj__markStack(mjData*) __attribute__((noinline)); -static inline void mj_markStack(mjData* d) __attribute__((always_inline)) { +void mj__markStack(mjData *) __attribute__((noinline)); +static inline void mj_markStack(mjData *d) __attribute__((always_inline)) { asm volatile("" ::: "memory"); mj__markStack(d); asm volatile("" ::: "memory"); } -void mj__freeStack(mjData*) __attribute__((noinline)); -static inline void mj_freeStack(mjData* d) __attribute__((always_inline)) { +void mj__freeStack(mjData *) __attribute__((noinline)); +static inline void mj_freeStack(mjData *d) __attribute__((always_inline)) { asm volatile("" ::: "memory"); mj__freeStack(d); asm volatile("" ::: "memory"); } -#endif // ADDRESS_SANITIZER +#endif // ADDRESS_SANITIZER #ifdef __cplusplus } #endif -#endif // MUJOCO_MUJOCO_H_ +#endif // MUJOCO_MUJOCO_H_ diff --git a/planner/locomotion/convex_mpc/CMakeLists.txt b/planner/locomotion/convex_mpc/CMakeLists.txt index e7939493..308eac8e 100644 --- a/planner/locomotion/convex_mpc/CMakeLists.txt +++ b/planner/locomotion/convex_mpc/CMakeLists.txt @@ -11,13 +11,13 @@ file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/convex_mpc/*.h*) #cmake_print_variables(${PROJECT_NAME}_SOURCES) add_library( - ${PROJECT_NAME} + ${PROJECT_NAME} SHARED - ${${PROJECT_NAME}_SOURCES} + ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS} ) target_link_libraries( - ${PROJECT_NAME} + ${PROJECT_NAME} PUBLIC #${PINOCCHIO_LIBRARIES} #pinocchio::pinocchio @@ -28,7 +28,7 @@ target_link_libraries( ) target_include_directories( - ${PROJECT_NAME} + ${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR} #${PINOCCHIO_INCLUDE_DIRS} @@ -40,7 +40,7 @@ target_include_directories( ) #if (OPTIMIZE_FOR_NATIVE) #target_compile_options( - #${PROJECT_NAME} + #${PROJECT_NAME} #PUBLIC #-march=native #) diff --git a/planner/locomotion/convex_mpc/include/convex_mpc/mpc_solution.hpp b/planner/locomotion/convex_mpc/include/convex_mpc/mpc_solution.hpp index e243d925..cd435ac5 100644 --- a/planner/locomotion/convex_mpc/include/convex_mpc/mpc_solution.hpp +++ b/planner/locomotion/convex_mpc/include/convex_mpc/mpc_solution.hpp @@ -3,8 +3,8 @@ #include "convex_mpc/contact_schedule.hpp" #include "convex_mpc/qp_data.hpp" -//#include "convex_mpc/robot_state.hpp" -//#include "convex_mpc/single_rigid_body.hpp" +// #include "convex_mpc/robot_state.hpp" +// #include "convex_mpc/single_rigid_body.hpp" #include "convex_mpc/types.hpp" #include "util/util.hpp" diff --git a/planner/locomotion/convex_mpc/test/CMakeLists.txt b/planner/locomotion/convex_mpc/test/CMakeLists.txt index 57d36a3c..52426691 100644 --- a/planner/locomotion/convex_mpc/test/CMakeLists.txt +++ b/planner/locomotion/convex_mpc/test/CMakeLists.txt @@ -4,11 +4,11 @@ include(cmake/gtest.cmake) # macto for testing macro(add_srbd_mpc_test TESTNAME) add_executable( - ${TESTNAME} + ${TESTNAME} ${TESTNAME}.cpp ) target_include_directories( - ${TESTNAME} + ${TESTNAME} PRIVATE ${GTEST_INCLUDE_PATH} ${GMOCK_INCLUDE_PATH} @@ -17,7 +17,7 @@ macro(add_srbd_mpc_test TESTNAME) ${PROJECT_SOURCE_DIR}/test/test_helper ) target_link_libraries( - ${TESTNAME} + ${TESTNAME} PRIVATE GTest::GTest GTest::GMock @@ -29,11 +29,11 @@ macro(add_srbd_mpc_test TESTNAME) endif() add_dependencies( - ${TESTNAME} + ${TESTNAME} googletest ) add_test( - NAME ${TESTNAME} + NAME ${TESTNAME} COMMAND $ ) endmacro() diff --git a/planner/locomotion/convex_mpc/test/cmake/gtest.cmake b/planner/locomotion/convex_mpc/test/cmake/gtest.cmake index 783a76f0..291be214 100644 --- a/planner/locomotion/convex_mpc/test/cmake/gtest.cmake +++ b/planner/locomotion/convex_mpc/test/cmake/gtest.cmake @@ -1,6 +1,6 @@ include(ExternalProject) -# find pthread for googletest +# find pthread for googletest find_package(Threads REQUIRED) SET_DIRECTORY_PROPERTIES(PROPERTIES EP_PREFIX ${CMAKE_BINARY_DIR}/external) @@ -34,4 +34,4 @@ set_target_properties(${GMOCK_LIBRARY} PROPERTIES IMPORTED_LOCATION ${GMOCK_LIBRARY_PATH} INTERFACE_LINK_LIBRARIES Threads::Threads ) -add_dependencies(${GMOCK_LIBRARY} googletest) \ No newline at end of file +add_dependencies(${GMOCK_LIBRARY} googletest) diff --git a/plot/convex_mpc.m b/plot/convex_mpc.m index 3813c402..c7eb18c1 100644 --- a/plot/convex_mpc.m +++ b/plot/convex_mpc.m @@ -21,7 +21,7 @@ time(1:row, i) = time(1:row, i) + time(end, i-1); end -time_vec = reshape(time, [row*col, 1]); +time_vec = reshape(time, [row*col, 1]); time_rf = time(2:end, 1:col); time_rf_vec = reshape(time_rf, [(row-1)*col, 1]); @@ -173,7 +173,7 @@ % plot(1:10, squeeze(crbi(1, 1, 51:60)),'-o'); % grid on % subplot(2,3,2); -% plot(1:10, squeeze(crbi(2,2, 51:60)), '-o'); +% plot(1:10, squeeze(crbi(2,2, 51:60)), '-o'); % grid on % subplot(2,3,3); % plot(1:10, squeeze(crbi(3, 3, 51:60)), '-o'); @@ -198,10 +198,10 @@ % Example number of time steps [row, col] = size(des_base_pos); n = col; -% +% % plot3(des_base_pos(1,:), des_base_pos(2,:), des_base_pos(3,:), 'k-', 'LineWidth',2); % hold on -% +% % plot3(des_lf_pos(1,:), des_lf_pos(2,:), des_lf_pos(3,:), 'r-', 'LineWidth',2); % plot3(des_rf_pos(1,:), des_rf_pos(2,:), des_rf_pos(3,:), 'b-', 'LineWidth',2); @@ -213,14 +213,14 @@ for i = 1:10:n-1 % Determine the end of the current segment endIndex = min(i+9, n); - + % Extract the current segment of the trajectory segmentX = des_lf_pos(1, i:endIndex); segmentY = des_lf_pos(2, i:endIndex); segmentZ = des_lf_pos(3, i:endIndex); segmentBaseX = des_base_pos(1, i:endIndex); - + % Determining the color index colorIndex = ceil(i/10); if colorIndex > size(colors, 1) @@ -252,4 +252,4 @@ set(gca, 'TickLabelInterpreter', 'latex') set(gca, 'FontSize', 30) set(gca, 'Color', 'white') -end \ No newline at end of file +end diff --git a/plot/draco/draco_data_manager_mpc.py b/plot/draco/draco_data_manager_mpc.py index 48eb18f7..89e931bc 100644 --- a/plot/draco/draco_data_manager_mpc.py +++ b/plot/draco/draco_data_manager_mpc.py @@ -2,13 +2,12 @@ import sys import os -import time import ruamel.yaml as yaml import numpy as np cwd = os.getcwd() -sys.path.append(cwd + '/build') +sys.path.append(cwd + "/build") sys.path.append(cwd) from messages.draco_pb2 import * @@ -21,7 +20,6 @@ import meshcat_shapes from plot import meshcat_utils as vis_tools -import json import argparse parser = argparse.ArgumentParser() @@ -90,8 +88,10 @@ def check_if_kf_estimator(kf_pos, est_pos): ##meshcat visualizer if args.b_visualize: model, collision_model, visual_model = pin.buildModelsFromUrdf( - "robot_model/draco/draco_modified.urdf", "robot_model/draco", - pin.JointModelFreeFlyer()) + "robot_model/draco/draco_modified.urdf", + "robot_model/draco", + pin.JointModelFreeFlyer(), + ) viz = MeshcatVisualizer(model, collision_model, visual_model) try: viz.initViewer(open=True) @@ -105,20 +105,20 @@ def check_if_kf_estimator(kf_pos, est_pos): vis_q = pin.neutral(model) # add other visualizations to viewer - meshcat_shapes.point(viz.viewer["com_des"], - color=vis_tools.Color.RED, - opacity=1.0, - radius=0.01) + meshcat_shapes.point( + viz.viewer["com_des"], color=vis_tools.Color.RED, opacity=1.0, radius=0.01 + ) - meshcat_shapes.point(viz.viewer["com"], - color=vis_tools.Color.BLUE, - opacity=1.0, - radius=0.01) + meshcat_shapes.point( + viz.viewer["com"], color=vis_tools.Color.BLUE, opacity=1.0, radius=0.01 + ) - meshcat_shapes.point(viz.viewer["com_projected"], - color=vis_tools.Color.BLACK, - opacity=1.0, - radius=0.01) + meshcat_shapes.point( + viz.viewer["com_projected"], + color=vis_tools.Color.BLACK, + opacity=1.0, + radius=0.01, + ) # add arrows visualizers to viewer vis_tools.add_arrow(viz.viewer, "grf_lf", color=[0, 0, 1]) ## BLUE @@ -129,7 +129,7 @@ def check_if_kf_estimator(kf_pos, est_pos): encoded_msg = socket.recv() msg.ParseFromString(encoded_msg) - #save data in pkl file + # save data in pkl file # data_saver.add('time', msg.time) # data_saver.add('phase', msg.phase) @@ -143,8 +143,12 @@ def check_if_kf_estimator(kf_pos, est_pos): if args.b_visualize: check_if_kf_estimator(msg.kf_base_joint_pos, msg.est_base_joint_pos) - base_pos = msg.kf_base_joint_pos if b_using_kf_estimator else msg.est_base_joint_pos - base_ori = msg.kf_base_joint_ori if b_using_kf_estimator else msg.est_base_joint_ori + base_pos = ( + msg.kf_base_joint_pos if b_using_kf_estimator else msg.est_base_joint_pos + ) + base_ori = ( + msg.kf_base_joint_ori if b_using_kf_estimator else msg.est_base_joint_ori + ) vis_q[0:3] = np.array(base_pos) vis_q[3:7] = np.array(base_ori) # quaternion [x,y,z,w] @@ -153,23 +157,28 @@ def check_if_kf_estimator(kf_pos, est_pos): viz.display(vis_q) trans = meshcat.transformations.translation_matrix( - [msg.des_com_pos[0], msg.des_com_pos[1], msg.des_com_pos[2]]) + [msg.des_com_pos[0], msg.des_com_pos[1], msg.des_com_pos[2]] + ) viz.viewer["com_des"].set_transform(trans) trans = meshcat.transformations.translation_matrix( - [msg.act_com_pos[0], msg.act_com_pos[1], msg.act_com_pos[2]]) + [msg.act_com_pos[0], msg.act_com_pos[1], msg.act_com_pos[2]] + ) viz.viewer["com"].set_transform(trans) trans = meshcat.transformations.translation_matrix( - [msg.des_com_pos[0], msg.des_com_pos[1], 0.0]) + [msg.des_com_pos[0], msg.des_com_pos[1], 0.0] + ) viz.viewer["com_projected"].set_transform(trans) # visualize GRFs if msg.phase != 1: - vis_tools.grf_display(viz.viewer["grf_lf"], msg.lfoot_pos, - msg.lfoot_ori, msg.lfoot_rf_cmd) - vis_tools.grf_display(viz.viewer["grf_rf"], msg.rfoot_pos, - msg.rfoot_ori, msg.rfoot_rf_cmd) + vis_tools.grf_display( + viz.viewer["grf_lf"], msg.lfoot_pos, msg.lfoot_ori, msg.lfoot_rf_cmd + ) + vis_tools.grf_display( + viz.viewer["grf_rf"], msg.rfoot_pos, msg.rfoot_ori, msg.rfoot_rf_cmd + ) ## visualize des CoM trajectory if len(msg.des_com_traj) > 0: @@ -178,14 +187,17 @@ def check_if_kf_estimator(kf_pos, est_pos): label = "des_com_" + str(i) des_com_traj_label.append(label) for name in des_com_traj_label: - meshcat_shapes.point(viz.viewer[name], - color=vis_tools.Color.GREEN, - opacity=0.8, - radius=0.01) + meshcat_shapes.point( + viz.viewer[name], + color=vis_tools.Color.GREEN, + opacity=0.8, + radius=0.01, + ) else: for com, name in zip(msg.des_com_traj, des_com_traj_label): trans = meshcat.transformations.translation_matrix( - [com.x, com.y, com.z]) + [com.x, com.y, com.z] + ) viz.viewer[name].set_transform(trans) ## visualize des orientation trajectory @@ -195,19 +207,23 @@ def check_if_kf_estimator(kf_pos, est_pos): label = "des_ori_" + str(i) des_ori_traj_label.append(label) for name in des_ori_traj_label: - meshcat_shapes.frame(viz.viewer[name], - axis_length=0.1, - axis_thickness=0.005, - opacity=0.8, - origin_radius=0.01) + meshcat_shapes.frame( + viz.viewer[name], + axis_length=0.1, + axis_thickness=0.005, + opacity=0.8, + origin_radius=0.01, + ) else: - for com, torso_ori, name in zip(msg.des_com_traj, - msg.des_torso_ori_traj, - des_ori_traj_label): + for com, torso_ori, name in zip( + msg.des_com_traj, msg.des_torso_ori_traj, des_ori_traj_label + ): rotation = meshcat.transformations.euler_matrix( - torso_ori.x, torso_ori.y, torso_ori.z, axes='sxyz') + torso_ori.x, torso_ori.y, torso_ori.z, axes="sxyz" + ) trans = meshcat.transformations.translation_matrix( - [com.x, com.y, com.z]) + [com.x, com.y, com.z] + ) viz.viewer[name].set_transform(trans.dot(rotation)) ## visualize left foot position trajectory @@ -217,15 +233,17 @@ def check_if_kf_estimator(kf_pos, est_pos): label = "des_lf_pos_" + str(i) des_lf_pos_traj_label.append(label) for name in des_lf_pos_traj_label: - meshcat_shapes.point(viz.viewer[name], - color=vis_tools.Color.CYAN, - opacity=0.8, - radius=0.01) + meshcat_shapes.point( + viz.viewer[name], + color=vis_tools.Color.CYAN, + opacity=0.8, + radius=0.01, + ) else: - for lf_pos, name in zip(msg.des_lf_pos_traj, - des_lf_pos_traj_label): + for lf_pos, name in zip(msg.des_lf_pos_traj, des_lf_pos_traj_label): trans = meshcat.transformations.translation_matrix( - [lf_pos.x, lf_pos.y, lf_pos.z]) + [lf_pos.x, lf_pos.y, lf_pos.z] + ) viz.viewer[name].set_transform(trans) ## visualize right foot position trajectory @@ -235,15 +253,17 @@ def check_if_kf_estimator(kf_pos, est_pos): label = "des_rf_pos_" + str(i) des_rf_pos_traj_label.append(label) for name in des_rf_pos_traj_label: - meshcat_shapes.point(viz.viewer[name], - color=vis_tools.Color.YELLOW, - opacity=0.8, - radius=0.01) + meshcat_shapes.point( + viz.viewer[name], + color=vis_tools.Color.YELLOW, + opacity=0.8, + radius=0.01, + ) else: - for rf_pos, name in zip(msg.des_rf_pos_traj, - des_rf_pos_traj_label): + for rf_pos, name in zip(msg.des_rf_pos_traj, des_rf_pos_traj_label): trans = meshcat.transformations.translation_matrix( - [rf_pos.x, rf_pos.y, rf_pos.z]) + [rf_pos.x, rf_pos.y, rf_pos.z] + ) viz.viewer[name].set_transform(trans) ## visualize left foot orientation trajectory @@ -253,19 +273,23 @@ def check_if_kf_estimator(kf_pos, est_pos): label = "des_lf_ori_" + str(i) des_lf_ori_traj_label.append(label) for name in des_lf_ori_traj_label: - meshcat_shapes.frame(viz.viewer[name], - axis_length=0.1, - axis_thickness=0.005, - opacity=0.8, - origin_radius=0.01) + meshcat_shapes.frame( + viz.viewer[name], + axis_length=0.1, + axis_thickness=0.005, + opacity=0.8, + origin_radius=0.01, + ) else: - for lf_pos, lf_ori, name in zip(msg.des_lf_pos_traj, - msg.des_lf_ori_traj, - des_lf_ori_traj_label): + for lf_pos, lf_ori, name in zip( + msg.des_lf_pos_traj, msg.des_lf_ori_traj, des_lf_ori_traj_label + ): rotation = meshcat.transformations.euler_matrix( - lf_ori.x, lf_ori.y, lf_ori.z, axes='sxyz') + lf_ori.x, lf_ori.y, lf_ori.z, axes="sxyz" + ) trans = meshcat.transformations.translation_matrix( - [lf_pos.x, lf_pos.y, lf_pos.z]) + [lf_pos.x, lf_pos.y, lf_pos.z] + ) viz.viewer[name].set_transform(trans.dot(rotation)) ## visualize right foot orientation trajectory @@ -275,17 +299,21 @@ def check_if_kf_estimator(kf_pos, est_pos): label = "des_rf_ori_" + str(i) des_rf_ori_traj_label.append(label) for name in des_rf_ori_traj_label: - meshcat_shapes.frame(viz.viewer[name], - axis_length=0.1, - axis_thickness=0.005, - opacity=0.8, - origin_radius=0.01) + meshcat_shapes.frame( + viz.viewer[name], + axis_length=0.1, + axis_thickness=0.005, + opacity=0.8, + origin_radius=0.01, + ) else: - for rf_pos, rf_ori, name in zip(msg.des_rf_pos_traj, - msg.des_rf_ori_traj, - des_rf_ori_traj_label): + for rf_pos, rf_ori, name in zip( + msg.des_rf_pos_traj, msg.des_rf_ori_traj, des_rf_ori_traj_label + ): rotation = meshcat.transformations.euler_matrix( - rf_ori.x, rf_ori.y, rf_ori.z, axes='sxyz') + rf_ori.x, rf_ori.y, rf_ori.z, axes="sxyz" + ) trans = meshcat.transformations.translation_matrix( - [rf_pos.x, rf_pos.y, rf_pos.z]) + [rf_pos.x, rf_pos.y, rf_pos.z] + ) viz.viewer[name].set_transform(trans.dot(rotation)) diff --git a/plot/draco/experiment_replay.py b/plot/draco/experiment_replay.py index b19f21bf..61781feb 100644 --- a/plot/draco/experiment_replay.py +++ b/plot/draco/experiment_replay.py @@ -61,34 +61,30 @@ vis_q = pin.neutral(model) # add other visualizations to viewer -com_des_viz, com_des_model = vis_tools.add_sphere(viz.viewer, - "com_des", - color=[0.0, 0.0, 1.0, 0.5]) +com_des_viz, com_des_model = vis_tools.add_sphere( + viz.viewer, "com_des", color=[0.0, 0.0, 1.0, 0.5] +) com_des_viz_q = pin.neutral(com_des_model) -com_viz, com_model = vis_tools.add_sphere(viz.viewer, - "com", - color=[1.0, 0.0, 0.0, 0.5]) +com_viz, com_model = vis_tools.add_sphere(viz.viewer, "com", color=[1.0, 0.0, 0.0, 0.5]) com_viz_q = pin.neutral(com_model) -com_proj_viz, com_proj_model = vis_tools.add_sphere(viz.viewer, - "com_proj", - color=[0.0, 0.0, 1.0, 0.3]) +com_proj_viz, com_proj_model = vis_tools.add_sphere( + viz.viewer, "com_proj", color=[0.0, 0.0, 1.0, 0.3] +) com_proj_viz_q = pin.neutral(com_proj_model) -icp_viz, icp_model = vis_tools.add_sphere(viz.viewer, - "icp", - color=vis_tools.violet) +icp_viz, icp_model = vis_tools.add_sphere(viz.viewer, "icp", color=vis_tools.violet) icp_viz_q = pin.neutral(icp_model) -icp_des_viz, icp_des_model = vis_tools.add_sphere(viz.viewer, - "icp_des", - color=[0.0, 1.0, 0.0, 0.3]) +icp_des_viz, icp_des_model = vis_tools.add_sphere( + viz.viewer, "icp_des", color=[0.0, 1.0, 0.0, 0.3] +) icp_des_viz_q = pin.neutral(icp_des_model) -cmp_des_viz, cmp_des_model = vis_tools.add_sphere(viz.viewer, - "cmp_des", - color=[0.0, 0.75, 0.75, 0.3]) +cmp_des_viz, cmp_des_model = vis_tools.add_sphere( + viz.viewer, "cmp_des", color=[0.0, 0.75, 0.75, 0.3] +) cmp_des_viz_q = pin.neutral(cmp_des_model) # add arrows visualizers to viewer @@ -107,18 +103,16 @@ try: cfg = yaml.load(stream, Loader=yaml.FullLoader) t_ini_footsteps_planned.append( - np.array(cfg["temporal_parameters"]["initial_time"])) + np.array(cfg["temporal_parameters"]["initial_time"]) + ) t_end_footsteps_planned.append( - np.array(cfg["temporal_parameters"]["final_time"])) - rfoot_contact_pos.append( - np.array(cfg["contact"]["right_foot"]["pos"])) - rfoot_contact_ori.append( - np.array(cfg["contact"]["right_foot"]["ori"])) + np.array(cfg["temporal_parameters"]["final_time"]) + ) + rfoot_contact_pos.append(np.array(cfg["contact"]["right_foot"]["pos"])) + rfoot_contact_ori.append(np.array(cfg["contact"]["right_foot"]["ori"])) # assert rfoot_contact_pos.shape[0] == rfoot_contact_ori.shape[0] - lfoot_contact_pos.append( - np.array(cfg["contact"]["left_foot"]["pos"])) - lfoot_contact_ori.append( - np.array(cfg["contact"]["left_foot"]["ori"])) + lfoot_contact_pos.append(np.array(cfg["contact"]["left_foot"]["pos"])) + lfoot_contact_ori.append(np.array(cfg["contact"]["left_foot"]["ori"])) # assert lfoot_contact_pos.shape[0] == lfoot_contact_ori.shape[0] except yaml.YAMLError as exc: print(exc) @@ -145,9 +139,9 @@ d = pickle.load(file) exp_time.append(d["time"]) phase.append(d["phase"]) - joint_positions.append(d["kf_base_joint_pos"] + - d["kf_base_joint_ori"] + - d["joint_positions"]) + joint_positions.append( + d["kf_base_joint_pos"] + d["kf_base_joint_ori"] + d["joint_positions"] + ) com_position_des.append(d["des_com_pos"]) com_position.append(d["act_com_pos"]) @@ -263,24 +257,25 @@ # save other visualizations at current frame vis_tools.display_visualizer_frames(viz, frame) # robot - vis_tools.display_visualizer_frames(com_proj_viz, - frame) # projected CoM - vis_tools.display_visualizer_frames(com_des_viz, - frame) # desired CoM position - vis_tools.display_visualizer_frames(com_viz, - frame) # actual CoM position + vis_tools.display_visualizer_frames(com_proj_viz, frame) # projected CoM + vis_tools.display_visualizer_frames(com_des_viz, frame) # desired CoM position + vis_tools.display_visualizer_frames(com_viz, frame) # actual CoM position vis_tools.display_visualizer_frames(icp_viz, frame) # actual ICP vis_tools.display_visualizer_frames(icp_des_viz, frame) # desired ICP vis_tools.display_visualizer_frames(cmp_des_viz, frame) # desired CMP - vis_tools.display_coordinate_frame(local_frame_name, - quat_world_local[ti], frame) + vis_tools.display_coordinate_frame( + local_frame_name, quat_world_local[ti], frame + ) # show footsteps ONLY if they have already been planned if b_show_footsteps and b_footsteps_available: - if (t_ini_footsteps_planned[curr_step_plan] < exp_time[ti] < - t_end_footsteps_planned[curr_step_plan] + 0.01): + if ( + t_ini_footsteps_planned[curr_step_plan] + < exp_time[ti] + < t_end_footsteps_planned[curr_step_plan] + 0.01 + ): footsteps_viz["lf_footsteps"].set_property("visible", True) footsteps_viz["rf_footsteps"].set_property("visible", True) vis_tools.update_footstep( @@ -310,7 +305,8 @@ # check if we need to update the index of step plans loaded if exp_time[ti] > t_end_footsteps_planned[ - curr_step_plan] and curr_step_plan < (footstep_plans - 1): + curr_step_plan + ] and curr_step_plan < (footstep_plans - 1): curr_step_plan += 1 frame_index = frame_index + 1 diff --git a/plot/foxglove_utils.py b/plot/foxglove_utils.py index ff3d0ac9..57e0b7b2 100644 --- a/plot/foxglove_utils.py +++ b/plot/foxglove_utils.py @@ -1,7 +1,4 @@ -import asyncio import json -import time -from foxglove_websocket.server import FoxgloveServerListener from foxglove_schemas_protobuf.SceneUpdate_pb2 import SceneUpdate @@ -86,4 +83,3 @@ def add_chan(self, server): "schema": self.schema, } ) - diff --git a/plot/meshcat_utils.py b/plot/meshcat_utils.py index 0c2baf8c..d0377f7f 100644 --- a/plot/meshcat_utils.py +++ b/plot/meshcat_utils.py @@ -10,40 +10,38 @@ class Color(object): - RED = 0xff0000 - GREEN = 0x00ff00 - BLUE = 0x0000ff + RED = 0xFF0000 + GREEN = 0x00FF00 + BLUE = 0x0000FF GREY = 0x888888 BLACK = 0x000000 - CYAN = 0x00ffff - YELLOW = 0xffff00 - VIOLET = 0x8f00ff + CYAN = 0x00FFFF + YELLOW = 0xFFFF00 + VIOLET = 0x8F00FF def add_arrow(meshcat_visualizer, obj_name, color=[1, 0, 0], height=0.1): arrow_shaft = g.Cylinder(height, 0.01) arrow_head = g.Cylinder(0.04, 0.04, radiusTop=0.001, radiusBottom=0.04) material = g.MeshPhongMaterial() - material.color = (int(color[0] * 255) * 256**2 + - int(color[1] * 255) * 256 + int(color[2] * 255)) + material.color = ( + int(color[0] * 255) * 256**2 + int(color[1] * 255) * 256 + int(color[2] * 255) + ) meshcat_visualizer[obj_name].set_object(arrow_shaft, material) meshcat_visualizer[obj_name]["head"].set_object(arrow_head, material) -def add_arrow_composite(meshcat_visualizer, - obj_name, - color=[1, 0, 0], - height=0.1): +def add_arrow_composite(meshcat_visualizer, obj_name, color=[1, 0, 0], height=0.1): arrow_shaft = g.Cylinder(height, 0.01) arrow_head = g.Cylinder(0.04, 0.04, radiusTop=0.001, radiusBottom=0.04) material = g.MeshPhongMaterial() - material.color = (int(color[0] * 255) * 256**2 + - int(color[1] * 255) * 256 + int(color[2] * 255)) + material.color = ( + int(color[0] * 255) * 256**2 + int(color[1] * 255) * 256 + int(color[2] * 255) + ) shaft_offset = tf.translation_matrix([0.0, height / 2.0, 0.0]) - meshcat_visualizer[obj_name]["arrow/shaft"].set_object( - arrow_shaft, material) + meshcat_visualizer[obj_name]["arrow/shaft"].set_object(arrow_shaft, material) meshcat_visualizer[obj_name]["arrow/head"].set_object(arrow_head, material) meshcat_visualizer[obj_name]["arrow/head"].set_transform(shaft_offset) @@ -59,14 +57,14 @@ def add_footsteps( # create footstep footstep = g.Box([foot_length, foot_width, 0.01]) material = g.MeshPhongMaterial() - material.color = (int(color[0] * 255) * 256**2 + - int(color[1] * 255) * 256 + int(color[2] * 255)) + material.color = ( + int(color[0] * 255) * 256**2 + int(color[1] * 255) * 256 + int(color[2] * 255) + ) material.opacity = 0.4 # add all footsteps to visualizer for step in range(footsteps_to_add): - meshcat_visualizer[obj_name]["step" + str(step)].set_object( - footstep, material) + meshcat_visualizer[obj_name]["step" + str(step)].set_object(footstep, material) def add_sphere( @@ -77,9 +75,11 @@ def add_sphere( color=[0.0, 0.0, 1.0, 0.5], ): sphere_model, sphere_collision_model, sphere_visual_model = pin.buildModelsFromUrdf( - urdf_path, visuals_path, pin.JointModelFreeFlyer()) - sphere_viz = MeshcatVisualizer(sphere_model, sphere_collision_model, - sphere_visual_model) + urdf_path, visuals_path, pin.JointModelFreeFlyer() + ) + sphere_viz = MeshcatVisualizer( + sphere_model, sphere_collision_model, sphere_visual_model + ) sphere_viz.initViewer(parent_visualizer) sphere_viz.loadViewerModel(rootNodeName=node_name, color=color) @@ -165,14 +165,16 @@ def display_visualizer_frames(meshcat_visualizer, frame): for visual in meshcat_visualizer.visual_model.geometryObjects: # Get mesh pose. M = meshcat_visualizer.visual_data.oMg[ - meshcat_visualizer.visual_model.getGeometryId(visual.name)] + meshcat_visualizer.visual_model.getGeometryId(visual.name) + ] # Manage scaling scale = np.asarray(visual.meshScale).flatten() S = np.diag(np.concatenate((scale, [1.0]))) T = np.array(M.homogeneous).dot(S) # Update viewer configuration. - frame[meshcat_visualizer.getViewerNodeName( - visual, pin.GeometryType.VISUAL)].set_transform(T) + frame[ + meshcat_visualizer.getViewerNodeName(visual, pin.GeometryType.VISUAL) + ].set_transform(T) def display_coordinate_frame(viz_name, frame_quat, viz_frame): diff --git a/scripts/draco_manipulation_comm.py b/scripts/draco_manipulation_comm.py index cc6a7823..569fe30a 100644 --- a/scripts/draco_manipulation_comm.py +++ b/scripts/draco_manipulation_comm.py @@ -10,7 +10,7 @@ import sys cwd = os.getcwd() -sys.path.append(cwd + '/build') +sys.path.append(cwd + "/build") from messages.draco_pb2 import * @@ -18,13 +18,13 @@ class DracoZMQServer(object): - def __init__(self, ip, sub_port, pub_port, logger_fn=print, verbose=False): - self.ip = ip self.pub_port = pub_port self.sub_port = sub_port - self._running = Value(ctypes.c_bool, ) + self._running = Value( + ctypes.c_bool, + ) self._init_time = 0.0 self._send_proc = None self._recv_proc = None @@ -47,15 +47,17 @@ def _publish(self): if not self._cmd: continue cmd = {key: value for key, value in self._cmd.items()} - self._msg.pos[:] = cmd['pos'] - self._msg.quat[:] = cmd['quat'] - self._msg.b_grasp = cmd['b_grasp'] - self._msg.b_teleop_toggled = cmd['b_teleop_toggled'] + self._msg.pos[:] = cmd["pos"] + self._msg.quat[:] = cmd["quat"] + self._msg.b_grasp = cmd["b_grasp"] + self._msg.b_teleop_toggled = cmd["b_teleop_toggled"] pub_socket.send(self._msg.SerializeToString()) if self._verbose: self._logger_fn( - "[{:.2f}] Publishing message...".format(time.time() - - self._init_time)) + "[{:.2f}] Publishing message...".format( + time.time() - self._init_time + ) + ) for key, val in self._cmd.items(): self._logger_fn("{}: {}".format(key, val)) self._cmd.clear() @@ -64,7 +66,6 @@ def _publish(self): self._logger_fn("Publishing Socket is closed...") def _subscribe(self): - context = zmq.Context() sub_socket = context.socket(zmq.SUB) sub_socket.setsockopt(zmq.SUBSCRIBE, b"") @@ -78,8 +79,8 @@ def _subscribe(self): self._obs.update(obs) if self._verbose: self._logger_fn( - "[{:.2f}] Recieved message...".format(time.time() - - self._init_time)) + "[{:.2f}] Recieved message...".format(time.time() - self._init_time) + ) for key, val in self._obs.items(): self._logger_fn("{}: {}".format(key, val)) @@ -87,7 +88,6 @@ def _subscribe(self): self._logger_fn("Subscription Socket is closed...") def start(self): - assert self._send_proc is None and self._recv_proc is None self._cmd.clear() @@ -96,8 +96,12 @@ def start(self): self._running.value = True self._init_time = time.time() - self._send_proc = Process(target=self._publish, ) - self._recv_proc = Process(target=self._subscribe, ) + self._send_proc = Process( + target=self._publish, + ) + self._recv_proc = Process( + target=self._subscribe, + ) self._send_proc.start() self._recv_proc.start() @@ -120,14 +124,14 @@ def obs(self): class DracoZMQClient(object): - def __init__(self, ip, sub_port, pub_port, logger_fn=print): - self.ip = ip self.pub_port = pub_port self.sub_port = sub_port - self._running = Value(ctypes.c_bool, ) + self._running = Value( + ctypes.c_bool, + ) self._init_time = 0.0 self._send_proc = None self._recv_proc = None @@ -139,7 +143,6 @@ def __init__(self, ip, sub_port, pub_port, logger_fn=print): self._logger_fn("TESTING SPOT LOGGING...") def _publish(self): - context = zmq.Context() pub_socket = context.socket(zmq.PUB) pub_socket.connect("tcp://{}:{}".format(self.ip, self.pub_port)) @@ -152,8 +155,8 @@ def _publish(self): msg = pickle.dumps(obs) pub_socket.send(msg) self._logger_fn( - "[{}] Publishing message...".format(time.time() - - self._init_time)) + "[{}] Publishing message...".format(time.time() - self._init_time) + ) self._logger_fn(self._obs.items()) self._obs.clear() @@ -161,7 +164,6 @@ def _publish(self): self._logger_fn("Publishing Socket is closed...") def _subscribe(self): - context = zmq.Context() sub_socket = context.socket(zmq.SUB) sub_socket.setsockopt(zmq.SUBSCRIBE, b"") @@ -173,15 +175,15 @@ def _subscribe(self): data = sub_socket.recv() cmd = pickle.loads(data) self._cmd.update(cmd) - self._logger_fn("[{}] Recieved message...".format(time.time() - - self._init_time)) + self._logger_fn( + "[{}] Recieved message...".format(time.time() - self._init_time) + ) self._logger_fn(self._cmd.items()) sub_socket.close() self._logger_fn("Subscription Socket is closed...") def start(self): - assert self._send_proc is None and self._recv_proc is None self._obs.clear() @@ -191,8 +193,12 @@ def start(self): self._running.value = True self._init_time = time.time() - self._send_proc = Process(target=self._publish, ) - self._recv_proc = Process(target=self._subscribe, ) + self._send_proc = Process( + target=self._publish, + ) + self._recv_proc = Process( + target=self._subscribe, + ) self._send_proc.start() self._recv_proc.start() diff --git a/scripts/real_teleop.py b/scripts/real_teleop.py index efb53d0e..f35075e0 100644 --- a/scripts/real_teleop.py +++ b/scripts/real_teleop.py @@ -11,11 +11,11 @@ from util.python_utils.device.t265 import T265 import util.python_utils.util as util -import copy import time import cv2 import csv from scipy.spatial.transform import Rotation as R + # from util.python_utils.comm import ZMQServer from scripts.draco_manipulation_comm import DracoZMQServer import util.python_utils.demo as demo @@ -27,14 +27,12 @@ FPS = 20 # Define colors for each axis -AXIS_COLORS = ['r', 'g', 'b'] # Red for x, Green for y, Blue for z +AXIS_COLORS = ["r", "g", "b"] # Red for x, Green for y, Blue for z ## Define the thread receiving keyboard for debugging -class Keyboard(): - +class Keyboard: def __init__(self): - self.single_click_and_hold = False self._reset_state = 0 @@ -48,8 +46,9 @@ def __init__(self): self._grasp = False # launch a new listener thread to listen to keyboard - self.thread = keyboard.Listener(on_press=self._on_press, - on_release=self._on_release) + self.thread = keyboard.Listener( + on_press=self._on_press, on_release=self._on_release + ) self.thread.start() def _reset_internal_state(self): @@ -64,46 +63,44 @@ def _reset_internal_state(self): self._t_click = -1 def _on_press(self, key): - try: key_char = key.char except AttributeError: key_char = str(key) - if key_char == 'e': + if key_char == "e": self._t_last_click = -1 self._t_click = time.time() elapsed_time = self._t_click - self._t_last_click self._t_last_click = self._t_click self.single_click_and_hold = True - elif key_char == 's': + elif key_char == "s": self._succeed = True - print('Recording successful') + print("Recording successful") def _on_release(self, key): - try: key_char = key.char except AttributeError: key_char = str(key) - if key_char == 'e': + if key_char == "e": self.single_click_and_hold = False - elif key == keyboard.Key.esc or key_char == 'q': + elif key == keyboard.Key.esc or key_char == "q": self._reset_state = 1 self._enabled = False self._reset_internal_state() - elif key_char == 'r': + elif key_char == "r": self._reset_state = 1 self._enabled = True - elif key_char == 'p': + elif key_char == "p": self._reset_state = 1 self._grasp = True - elif key_char == 'o': + elif key_char == "o": self._reset_state = 1 self._grasp = False @@ -132,11 +129,10 @@ def grasp(self): def record(path, ros_socket=None): - t265 = T265() keyboard = Keyboard() mat_trans = np.eye(4) - mat_trans[:3, :3] = util.quat_to_rot([0.5, -0.5, -0.5, 0.5]) #T265 + mat_trans[:3, :3] = util.quat_to_rot([0.5, -0.5, -0.5, 0.5]) # T265 trk_init = True done = False @@ -153,16 +149,18 @@ def record(path, ros_socket=None): os.makedirs(dir_path, exist_ok=True) if t265.b_img_stream: - left_img_path = os.path.join(dir_path, 'left_img') - right_img_path = os.path.join(dir_path, 'right_img') + left_img_path = os.path.join(dir_path, "left_img") + right_img_path = os.path.join(dir_path, "right_img") os.makedirs(left_img_path, exist_ok=True) os.makedirs(right_img_path, exist_ok=True) - low_dim_file = open(os.path.join(dir_path, 'low_dim.csv'), 'w') - low_dim_file.write("time, \ + low_dim_file = open(os.path.join(dir_path, "low_dim.csv"), "w") + low_dim_file.write( + "time, \ pos_x, pos_y, pos_z, \ quat_x, quat_y, quat_z, quat_w, \ - \n") + \n" + ) # low_dim_file.write("time, \ # pos_x, pos_y, pos_z, \ # quat_x, quat_y, quat_z, quat_w, \ @@ -176,7 +174,6 @@ def record(path, ros_socket=None): quat = np.array([0, 0, 0, 1]) while not done: - if t265.time > read_time + 0.1: trk_pos = t265.pos trk_rot = t265.rot @@ -195,8 +192,7 @@ def record(path, ros_socket=None): if trk_init: mat_se3_base = np.eye(4) - mat_se3_base = np.linalg.inv( - mat_trans @ mat_se3) @ mat_se3_base + mat_se3_base = np.linalg.inv(mat_trans @ mat_se3) @ mat_se3_base trk_init = False print("Tracking initialized... Start now") @@ -206,12 +202,14 @@ def record(path, ros_socket=None): b_grasp = keyboard.grasp b_teleop_toggled = keyboard.enable - ros_socket.update({ - "pos": pos, - "quat": quat, - "b_grasp": b_grasp, - "b_teleop_toggled": b_teleop_toggled - }) + ros_socket.update( + { + "pos": pos, + "quat": quat, + "b_grasp": b_grasp, + "b_teleop_toggled": b_teleop_toggled, + } + ) # state['time'] += [t265.time] # state['left_img'] += [[left_img]] @@ -225,30 +223,33 @@ def record(path, ros_socket=None): done = not keyboard.enable if not trk_init: - - low_dim_file.write("{}, \ + low_dim_file.write( + "{}, \ {}, {}, {},\ {}, {}, {}, {},\ \n".format( - t265.time, - pos[0], - pos[1], - pos[2], - quat[0], - quat[1], - quat[2], - quat[3], - )) + t265.time, + pos[0], + pos[1], + pos[2], + quat[0], + quat[1], + quat[2], + quat[3], + ) + ) if t265.b_img_stream: cv2.imwrite( - os.path.join(left_img_path, - "{}.png".format(int(t265.time * 100))), + os.path.join( + left_img_path, "{}.png".format(int(t265.time * 100)) + ), left_img, ) cv2.imwrite( - os.path.join(right_img_path, - "{}.png".format(int(t265.time * 100))), + os.path.join( + right_img_path, "{}.png".format(int(t265.time * 100)) + ), right_img, ) @@ -259,11 +260,10 @@ def record(path, ros_socket=None): def visualize(data_path): - - low_dim_file = open(os.path.join(data_path, 'low_dim.csv'), 'r') + low_dim_file = open(os.path.join(data_path, "low_dim.csv"), "r") low_dim_reader = csv.reader(low_dim_file) - left_img_dir = os.path.join(data_path, 'left_img') - right_img_dir = os.path.join(data_path, 'right_img') + left_img_dir = os.path.join(data_path, "left_img") + right_img_dir = os.path.join(data_path, "right_img") # first row is the header # create a dict with the header names as keys and save values of the lines to corresponding keys @@ -277,21 +277,15 @@ def visualize(data_path): next(low_dim_reader) for row in low_dim_reader: - data['time'] += [float(row[0])] - data['trk_pos'] += [[float(row[1]), float(row[2]), float(row[3])]] - data['trk_rot'] += [[ - float(row[4]), - float(row[5]), - float(row[6]), - float(row[7]) - ]] + data["time"] += [float(row[0])] + data["trk_pos"] += [[float(row[1]), float(row[2]), float(row[3])]] + data["trk_rot"] += [ + [float(row[4]), float(row[5]), float(row[6]), float(row[7])] + ] data["robot_pos"] += [[float(row[8]), float(row[9]), float(row[10])]] - data["robot_rot"] += [[ - float(row[11]), - float(row[12]), - float(row[13]), - float(row[14]) - ]] + data["robot_rot"] += [ + [float(row[11]), float(row[12]), float(row[13]), float(row[14])] + ] for key in data.keys(): data[key] = np.array(data[key]) @@ -300,17 +294,19 @@ def visualize(data_path): mat_tool_offset[:3, :3] = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) mat_tool_offset[:3, 3] = np.zeros(3) - data['left_img'] = [] - data['right_img'] = [] + data["left_img"] = [] + data["right_img"] = [] left_img = np.zeros((800, 848, 3), dtype="uint8") right_img = np.zeros((800, 848, 3), dtype="uint8") - for time in data['time']: + for time in data["time"]: left_img_read = cv2.imread( - os.path.join(left_img_dir, "{}.png".format(int(time * 100)))) + os.path.join(left_img_dir, "{}.png".format(int(time * 100))) + ) right_img_read = cv2.imread( - os.path.join(right_img_dir, "{}.png".format(int(time * 100)))) - data['left_img'] += [left_img] - data['right_img'] += [right_img] + os.path.join(right_img_dir, "{}.png".format(int(time * 100))) + ) + data["left_img"] += [left_img] + data["right_img"] += [right_img] # print(right_img) if left_img_read is not None: left_img = left_img_read @@ -318,9 +314,9 @@ def visualize(data_path): right_img = right_img_read # print(left_img.shape, right_img.shape) - time = np.array(data['time'][:]) - left_img = np.array(data['left_img'][:]) - right_img = np.array(data['right_img'][:]) + time = np.array(data["time"][:]) + left_img = np.array(data["left_img"][:]) + right_img = np.array(data["right_img"][:]) trk_pos = np.array(data["trk_pos"][:]) trk_rot = np.array(data["trk_rot"][:]) # ipdb.set_trace() @@ -348,7 +344,7 @@ def visualize(data_path): for i in range(trk_pos.shape[0] - 1): pose_vec_cur = np.concatenate([trk_pos[i + 1], trk_rot[i + 1]]) pose_vec_prv = np.concatenate([trk_pos[i], trk_rot[i]]) - action = demo.reconstruct_pose(pose_vec_cur, pose_vec_prv, mode='quat') + action = demo.reconstruct_pose(pose_vec_cur, pose_vec_prv, mode="quat") trk_action.append(action) trk_action = np.array(trk_action) @@ -356,20 +352,20 @@ def visualize(data_path): for i in range(cmd_pos.shape[0] - 1): pose_vec_cur = np.concatenate([cmd_pos[i + 1], cmd_rot[i + 1]]) pose_vec_prv = np.concatenate([cmd_pos[i], cmd_rot[i]]) - action = demo.reconstruct_pose(pose_vec_cur, pose_vec_prv, mode='quat') + action = demo.reconstruct_pose(pose_vec_cur, pose_vec_prv, mode="quat") cmd_action.append(action) cmd_action = np.array(cmd_action) estimated_pos = [] estimated_rot = [] pose_ee_vector_prv = np.concatenate( - [mat_tool_offset[:3, 3], - util.rot_to_quat(mat_tool_offset[:3, :3])]) + [mat_tool_offset[:3, 3], util.rot_to_quat(mat_tool_offset[:3, :3])] + ) for i in range(trk_action.shape[0] - 1): delta_pose_vector = trk_action[i] - pose_ee_vector_cur = demo.reconstruct_pose(delta_pose_vector, - pose_ee_vector_prv, - mode="quat") + pose_ee_vector_cur = demo.reconstruct_pose( + delta_pose_vector, pose_ee_vector_prv, mode="quat" + ) cam_pose_mat = np.eye(4) cam_pose_mat[:3, :3] = util.quat_to_rot(pose_ee_vector_cur[3:]) cam_pose_mat[:3, 3] = pose_ee_vector_cur[:3] @@ -382,13 +378,13 @@ def visualize(data_path): reconstructed_pos = [] reconstructed_rot = [] pose_ee_vector_prv = np.concatenate( - [mat_tool_offset[:3, 3], - util.rot_to_quat(mat_tool_offset[:3, :3])]) + [mat_tool_offset[:3, 3], util.rot_to_quat(mat_tool_offset[:3, :3])] + ) for i in range(cmd_action.shape[0] - 1): delta_pose_vector = cmd_action[i] - pose_ee_vector_cur = demo.reconstruct_pose(delta_pose_vector, - pose_ee_vector_prv, - mode="quat") + pose_ee_vector_cur = demo.reconstruct_pose( + delta_pose_vector, pose_ee_vector_prv, mode="quat" + ) cam_pose_mat = np.eye(4) cam_pose_mat[:3, :3] = util.quat_to_rot(pose_ee_vector_cur[3:]) cam_pose_mat[:3, 3] = pose_ee_vector_cur[:3] @@ -401,8 +397,8 @@ def visualize(data_path): fig = plt.figure() ax1 = fig.add_subplot(221) ax2 = fig.add_subplot(222) - ax3 = fig.add_subplot(223, projection='3d') - ax4 = fig.add_subplot(224, projection='3d') + ax3 = fig.add_subplot(223, projection="3d") + ax4 = fig.add_subplot(224, projection="3d") length = time.shape[0] img_buffer = [] @@ -418,45 +414,47 @@ def get_img_from_fig(fig, dpi=360): return img for num in range(length): - # Prepare the 2D plot for visual observation - pos_data_3 = estimated_pos[:num + 1, :] - rot_data_3 = estimated_rot[:num + 1, :] + pos_data_3 = estimated_pos[: num + 1, :] + rot_data_3 = estimated_rot[: num + 1, :] - pos_data_4 = reconstructed_pos[:num + 1, :] - rot_data_4 = reconstructed_rot[:num + 1, :] + pos_data_4 = reconstructed_pos[: num + 1, :] + rot_data_4 = reconstructed_rot[: num + 1, :] left_data = left_img[num] right_data = right_img[num] - fig.suptitle('Time: %.2f' % time[num]) + fig.suptitle("Time: %.2f" % time[num]) fig.set_size_inches(5, 2.5) if num == 0: # fig.set_size_inches(15, 7) left_image = ax1.imshow(left_img[0]) - ax1.set_title('Left image', fontdict={'fontsize': 8}) - ax1.tick_params(left=False, - right=False, - labelleft=False, - labelbottom=False, - bottom=False) + ax1.set_title("Left image", fontdict={"fontsize": 8}) + ax1.tick_params( + left=False, + right=False, + labelleft=False, + labelbottom=False, + bottom=False, + ) right_image = ax2.imshow(right_img[0]) - ax2.set_title('Right image', fontdict={'fontsize': 8}) - ax2.tick_params(left=False, - right=False, - labelleft=False, - labelbottom=False, - bottom=False) - - line_3, = ax3.plot(pos_data_3[0, 0], - pos_data_3[0, 1], - pos_data_3[0, 2], - color='blue') + ax2.set_title("Right image", fontdict={"fontsize": 8}) + ax2.tick_params( + left=False, + right=False, + labelleft=False, + labelbottom=False, + bottom=False, + ) + + (line_3,) = ax3.plot( + pos_data_3[0, 0], pos_data_3[0, 1], pos_data_3[0, 2], color="blue" + ) # point_3, = ax3.plot([], [], [], 'go') axis_3 = [] for j in range(3): - axis_j, = ax3.plot([], [], [], color=AXIS_COLORS[j]) + (axis_j,) = ax3.plot([], [], [], color=AXIS_COLORS[j]) axis_3.append(axis_j) ax3.set_xlim(pos_data_3[0, 0] - 0.6, pos_data_3[0, 0] + 0.6) ax3.set_ylim(pos_data_3[0, 1] - 0.6, pos_data_3[0, 1] + 0.6) @@ -464,17 +462,16 @@ def get_img_from_fig(fig, dpi=360): ax3.xaxis.set_tick_params(labelsize=4, pad=0) ax3.yaxis.set_tick_params(labelsize=4, pad=0) ax3.zaxis.set_tick_params(labelsize=4, pad=0) - ax3.set_title('Tracker trajectory', fontdict={'fontsize': 8}) + ax3.set_title("Tracker trajectory", fontdict={"fontsize": 8}) ax3.grid(True) - line_4, = ax4.plot(pos_data_4[0, 0], - pos_data_4[0, 1], - pos_data_4[0, 2], - color='blue') + (line_4,) = ax4.plot( + pos_data_4[0, 0], pos_data_4[0, 1], pos_data_4[0, 2], color="blue" + ) # point_4, = ax4.plot([], [], [], 'go') axis_4 = [] for j in range(3): - axis_j, = ax4.plot([], [], [], color=AXIS_COLORS[j]) + (axis_j,) = ax4.plot([], [], [], color=AXIS_COLORS[j]) axis_4.append(axis_j) ax4.set_xlim(pos_data_4[0, 0] - 0.6, pos_data_4[0, 0] + 0.6) ax4.set_ylim(pos_data_4[0, 1] - 0.6, pos_data_4[0, 1] + 0.6) @@ -482,7 +479,7 @@ def get_img_from_fig(fig, dpi=360): ax4.xaxis.set_tick_params(labelsize=4, pad=0) ax4.yaxis.set_tick_params(labelsize=4, pad=0) ax4.zaxis.set_tick_params(labelsize=4, pad=0) - ax4.set_title('Command trajectory', fontdict={'fontsize': 8}) + ax4.set_title("Command trajectory", fontdict={"fontsize": 8}) ax4.grid(True) left_image.set_array(left_data) @@ -497,8 +494,9 @@ def get_img_from_fig(fig, dpi=360): for j in range(3): start_point = pos_data_3[-1] end_point = pos_data_3[-1] + 0.1 * basis_3[j] - axis_3[j].set_data([start_point[0], end_point[0]], - [start_point[1], end_point[1]]) + axis_3[j].set_data( + [start_point[0], end_point[0]], [start_point[1], end_point[1]] + ) axis_3[j].set_3d_properties([start_point[2], end_point[2]]) # point_3.set_data(pos_data_3[-1,0], pos_data_3[-1,1]) # point_3.set_3d_properties(pos_data_3[-1,2]) @@ -512,8 +510,9 @@ def get_img_from_fig(fig, dpi=360): for j in range(3): start_point = pos_data_4[-1] end_point = pos_data_4[-1] + 0.1 * basis_4[j] - axis_4[j].set_data([start_point[0], end_point[0]], - [start_point[1], end_point[1]]) + axis_4[j].set_data( + [start_point[0], end_point[0]], [start_point[1], end_point[1]] + ) axis_4[j].set_3d_properties([start_point[2], end_point[2]]) # point_4.set_data(pos_data_4[-1,0], pos_data_4[-1,1]) # point_4.set_3d_properties(pos_data_4[-1,2]) @@ -522,24 +521,23 @@ def get_img_from_fig(fig, dpi=360): img_buffer.append(img) save_path = os.path.join(data_path, "visualize_trk.mp4") - video_writer = cv2.VideoWriter(save_path, cv2.VideoWriter_fourcc(*'mp4v'), - 30, img_buffer[0].shape[1::-1]) + video_writer = cv2.VideoWriter( + save_path, cv2.VideoWriter_fourcc(*"mp4v"), 30, img_buffer[0].shape[1::-1] + ) for img in img_buffer: video_writer.write(img) video_writer.release() if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument('--path', type=str, default='./data/test') + parser.add_argument("--path", type=str, default="./data/test") args = parser.parse_args() path = args.path - server = DracoZMQServer(ip=TARGET_IP, - pub_port=PUB_PORT, - sub_port=SUB_PORT, - verbose=True) + server = DracoZMQServer( + ip=TARGET_IP, pub_port=PUB_PORT, sub_port=SUB_PORT, verbose=True + ) server.start() demo_cnt = 1 diff --git a/simulator/mujoco/draco/simulate/array_safety.h b/simulator/mujoco/draco/simulate/array_safety.h index e8dd0d73..8e31e10b 100644 --- a/simulator/mujoco/draco/simulate/array_safety.h +++ b/simulator/mujoco/draco/simulate/array_safety.h @@ -21,12 +21,13 @@ #include #include -// Provides safe alternatives to the sizeof() operator and standard library functions for handling -// null-terminated (C-style) strings in raw char arrays. +// Provides safe alternatives to the sizeof() operator and standard library +// functions for handling null-terminated (C-style) strings in raw char arrays. // -// These functions make use of compile-time array sizes to limit read and write operations to within -// the array bounds. They are designed to trigger a compile error if the array size cannot be -// determined at compile time (e.g. when an array has decayed into a pointer). +// These functions make use of compile-time array sizes to limit read and write +// operations to within the array bounds. They are designed to trigger a compile +// error if the array size cannot be determined at compile time (e.g. when an +// array has decayed into a pointer). // // They do not perform runtime bound checks. @@ -36,7 +37,7 @@ namespace sample_util { // returns sizeof(arr) // use instead of sizeof() to avoid unintended array-to-pointer decay template -static constexpr std::size_t sizeof_arr(const T(&arr)[N]) { +static constexpr std::size_t sizeof_arr(const T (&arr)[N]) { return sizeof(arr); } @@ -61,7 +62,7 @@ static inline std::size_t strlen_arr(const char (&str)[N]) { // like std::sprintf but will not write beyond the bound of dest // dest is guaranteed to be null-terminated template -static inline int sprintf_arr(char (&dest)[N], const char* format, ...) { +static inline int sprintf_arr(char (&dest)[N], const char *format, ...) { std::va_list vargs; va_start(vargs, format); int retval = std::vsnprintf(dest, N, format, vargs); @@ -72,7 +73,7 @@ static inline int sprintf_arr(char (&dest)[N], const char* format, ...) { // like std::strcat but will not write beyond the bound of dest // dest is guaranteed to be null-terminated template -static inline char* strcat_arr(char (&dest)[N], const char* src) { +static inline char *strcat_arr(char (&dest)[N], const char *src) { const std::size_t dest_len = strlen_arr(dest); const std::size_t dest_size = sizeof_arr(dest); for (std::size_t i = dest_len; i < dest_size; ++i) { @@ -88,7 +89,7 @@ static inline char* strcat_arr(char (&dest)[N], const char* src) { // like std::strcpy but won't write beyond the bound of dest // dest is guaranteed to be null-terminated template -static inline char* strcpy_arr(char (&dest)[N], const char* src) { +static inline char *strcpy_arr(char (&dest)[N], const char *src) { { std::size_t i = 0; for (; src[i] && i < N - 1; ++i) { @@ -99,7 +100,7 @@ static inline char* strcpy_arr(char (&dest)[N], const char* src) { return &dest[0]; } -} // namespace sample_util -} // namespace mujoco +} // namespace sample_util +} // namespace mujoco -#endif // MUJOCO_SAMPLE_ARRAY_SAFETY_H_ +#endif // MUJOCO_SAMPLE_ARRAY_SAFETY_H_ diff --git a/simulator/mujoco/draco/simulate/glfw_adapter.cc b/simulator/mujoco/draco/simulate/glfw_adapter.cc index 825f6c1d..bb86688e 100644 --- a/simulator/mujoco/draco/simulate/glfw_adapter.cc +++ b/simulator/mujoco/draco/simulate/glfw_adapter.cc @@ -74,10 +74,11 @@ GlfwAdapter::GlfwAdapter() { window_, +[](GLFWwindow *window, int count, const char **paths) { GlfwAdapterFromWindow(window).OnFilesDrop(count, paths); }); - Glfw().glfwSetKeyCallback(window_, +[](GLFWwindow *window, int key, - int scancode, int act, int mods) { - GlfwAdapterFromWindow(window).OnKey(key, scancode, act); - }); + Glfw().glfwSetKeyCallback( + window_, + +[](GLFWwindow *window, int key, int scancode, int act, int mods) { + GlfwAdapterFromWindow(window).OnKey(key, scancode, act); + }); Glfw().glfwSetMouseButtonCallback( window_, +[](GLFWwindow *window, int button, int act, int mods) { GlfwAdapterFromWindow(window).OnMouseButton(button, act); @@ -90,15 +91,16 @@ GlfwAdapter::GlfwAdapter() { window_, +[](GLFWwindow *window, double xoffset, double yoffset) { GlfwAdapterFromWindow(window).OnScroll(xoffset, yoffset); }); - Glfw().glfwSetWindowRefreshCallback(window_, +[](GLFWwindow *window) { + Glfw().glfwSetWindowRefreshCallback( + window_, +[](GLFWwindow *window) { #ifdef __APPLE__ - auto &core_video = GlfwAdapterFromWindow(window).core_video_; - if (core_video.has_value()) { - core_video->UpdateDisplayLink(); - } + auto &core_video = GlfwAdapterFromWindow(window).core_video_; + if (core_video.has_value()) { + core_video->UpdateDisplayLink(); + } #endif - GlfwAdapterFromWindow(window).OnWindowRefresh(); - }); + GlfwAdapterFromWindow(window).OnWindowRefresh(); + }); Glfw().glfwSetWindowSizeCallback( window_, +[](GLFWwindow *window, int width, int height) { GlfwAdapterFromWindow(window).OnWindowResize(width, height); diff --git a/simulator/mujoco/draco/simulate/glfw_dispatch.h b/simulator/mujoco/draco/simulate/glfw_dispatch.h index f3bec97e..2bc0bbbe 100644 --- a/simulator/mujoco/draco/simulate/glfw_dispatch.h +++ b/simulator/mujoco/draco/simulate/glfw_dispatch.h @@ -71,7 +71,7 @@ struct Glfw { #undef mjGLFW_DECLARE_SYMBOL }; -const struct Glfw& Glfw(void* dlhandle = nullptr); -} // namespace mujoco +const struct Glfw &Glfw(void *dlhandle = nullptr); +} // namespace mujoco -#endif // MUJOCO_SIMULATE_GLFW_DISPATCH_H_ +#endif // MUJOCO_SIMULATE_GLFW_DISPATCH_H_ diff --git a/simulator/mujoco/draco/simulate/lodepng.cpp b/simulator/mujoco/draco/simulate/lodepng.cpp index f5d5a2d1..f5f73d28 100644 --- a/simulator/mujoco/draco/simulate/lodepng.cpp +++ b/simulator/mujoco/draco/simulate/lodepng.cpp @@ -357,7 +357,7 @@ static void lodepng_set32bitInt(unsigned char *buffer, unsigned value) { buffer[0] = (unsigned char)((value >> 24) & 0xff); buffer[1] = (unsigned char)((value >> 16) & 0xff); buffer[2] = (unsigned char)((value >> 8) & 0xff); - buffer[3] = (unsigned char)((value)&0xff); + buffer[3] = (unsigned char)((value) & 0xff); } #endif /*defined(LODEPNG_COMPILE_PNG) || defined(LODEPNG_COMPILE_ENCODER)*/ diff --git a/simulator/mujoco/draco/simulate/lodepng.h b/simulator/mujoco/draco/simulate/lodepng.h index 3f3649dc..e87cffba 100644 --- a/simulator/mujoco/draco/simulate/lodepng.h +++ b/simulator/mujoco/draco/simulate/lodepng.h @@ -28,7 +28,7 @@ freely, subject to the following restrictions: #include /*for size_t*/ -extern const char* LODEPNG_VERSION_STRING; +extern const char *LODEPNG_VERSION_STRING; /* The following #defines are used to create code sections. They can be disabled @@ -40,35 +40,41 @@ compiler command to disable them without modifying this header, e.g. /*deflate & zlib. If disabled, you must specify alternative zlib functions in the custom_zlib field of the compress and decompress settings*/ #ifndef LODEPNG_NO_COMPILE_ZLIB -/*pass -DLODEPNG_NO_COMPILE_ZLIB to the compiler to disable this, or comment out LODEPNG_COMPILE_ZLIB below*/ +/*pass -DLODEPNG_NO_COMPILE_ZLIB to the compiler to disable this, or comment out + * LODEPNG_COMPILE_ZLIB below*/ #define LODEPNG_COMPILE_ZLIB #endif /*png encoder and png decoder*/ #ifndef LODEPNG_NO_COMPILE_PNG -/*pass -DLODEPNG_NO_COMPILE_PNG to the compiler to disable this, or comment out LODEPNG_COMPILE_PNG below*/ +/*pass -DLODEPNG_NO_COMPILE_PNG to the compiler to disable this, or comment out + * LODEPNG_COMPILE_PNG below*/ #define LODEPNG_COMPILE_PNG #endif /*deflate&zlib decoder and png decoder*/ #ifndef LODEPNG_NO_COMPILE_DECODER -/*pass -DLODEPNG_NO_COMPILE_DECODER to the compiler to disable this, or comment out LODEPNG_COMPILE_DECODER below*/ +/*pass -DLODEPNG_NO_COMPILE_DECODER to the compiler to disable this, or comment + * out LODEPNG_COMPILE_DECODER below*/ #define LODEPNG_COMPILE_DECODER #endif /*deflate&zlib encoder and png encoder*/ #ifndef LODEPNG_NO_COMPILE_ENCODER -/*pass -DLODEPNG_NO_COMPILE_ENCODER to the compiler to disable this, or comment out LODEPNG_COMPILE_ENCODER below*/ +/*pass -DLODEPNG_NO_COMPILE_ENCODER to the compiler to disable this, or comment + * out LODEPNG_COMPILE_ENCODER below*/ #define LODEPNG_COMPILE_ENCODER #endif /*the optional built in harddisk file loading and saving functions*/ #ifndef LODEPNG_NO_COMPILE_DISK -/*pass -DLODEPNG_NO_COMPILE_DISK to the compiler to disable this, or comment out LODEPNG_COMPILE_DISK below*/ +/*pass -DLODEPNG_NO_COMPILE_DISK to the compiler to disable this, or comment out + * LODEPNG_COMPILE_DISK below*/ #define LODEPNG_COMPILE_DISK #endif -/*support for chunks other than IHDR, IDAT, PLTE, tRNS, IEND: ancillary and unknown chunks*/ +/*support for chunks other than IHDR, IDAT, PLTE, tRNS, IEND: ancillary and + * unknown chunks*/ #ifndef LODEPNG_NO_COMPILE_ANCILLARY_CHUNKS /*pass -DLODEPNG_NO_COMPILE_ANCILLARY_CHUNKS to the compiler to disable this, or comment out LODEPNG_COMPILE_ANCILLARY_CHUNKS below*/ @@ -82,53 +88,57 @@ or comment out LODEPNG_COMPILE_ERROR_TEXT below*/ #define LODEPNG_COMPILE_ERROR_TEXT #endif -/*Compile the default allocators (C's free, malloc and realloc). If you disable this, -you can define the functions lodepng_free, lodepng_malloc and lodepng_realloc in your -source files with custom allocators.*/ +/*Compile the default allocators (C's free, malloc and realloc). If you disable +this, you can define the functions lodepng_free, lodepng_malloc and +lodepng_realloc in your source files with custom allocators.*/ #ifndef LODEPNG_NO_COMPILE_ALLOCATORS -/*pass -DLODEPNG_NO_COMPILE_ALLOCATORS to the compiler to disable the built-in ones, -or comment out LODEPNG_COMPILE_ALLOCATORS below*/ +/*pass -DLODEPNG_NO_COMPILE_ALLOCATORS to the compiler to disable the built-in +ones, or comment out LODEPNG_COMPILE_ALLOCATORS below*/ #define LODEPNG_COMPILE_ALLOCATORS #endif /*Disable built-in CRC function, in that case a custom implementation of lodepng_crc32 must be defined externally so that it can be linked in. -The default built-in CRC code comes with 8KB of lookup tables, so for memory constrained environment you may want it -disabled and provide a much smaller implementation externally as said above. You can find such an example implementation -in a comment in the lodepng.c(pp) file in the 'else' case of the searchable LODEPNG_COMPILE_CRC section.*/ +The default built-in CRC code comes with 8KB of lookup tables, so for memory +constrained environment you may want it disabled and provide a much smaller +implementation externally as said above. You can find such an example +implementation in a comment in the lodepng.c(pp) file in the 'else' case of the +searchable LODEPNG_COMPILE_CRC section.*/ #ifndef LODEPNG_NO_COMPILE_CRC /*pass -DLODEPNG_NO_COMPILE_CRC to the compiler to disable the built-in one, or comment out LODEPNG_COMPILE_CRC below*/ #define LODEPNG_COMPILE_CRC #endif -/*compile the C++ version (you can disable the C++ wrapper here even when compiling for C++)*/ +/*compile the C++ version (you can disable the C++ wrapper here even when + * compiling for C++)*/ #ifdef __cplusplus #ifndef LODEPNG_NO_COMPILE_CPP -/*pass -DLODEPNG_NO_COMPILE_CPP to the compiler to disable C++ (not needed if a C-only compiler), -or comment out LODEPNG_COMPILE_CPP below*/ +/*pass -DLODEPNG_NO_COMPILE_CPP to the compiler to disable C++ (not needed if a +C-only compiler), or comment out LODEPNG_COMPILE_CPP below*/ #define LODEPNG_COMPILE_CPP #endif #endif #ifdef LODEPNG_COMPILE_CPP -#include #include +#include #endif /*LODEPNG_COMPILE_CPP*/ #ifdef LODEPNG_COMPILE_PNG /*The PNG color types (also used for raw image).*/ typedef enum LodePNGColorType { - LCT_GREY = 0, /*grayscale: 1,2,4,8,16 bit*/ - LCT_RGB = 2, /*RGB: 8,16 bit*/ - LCT_PALETTE = 3, /*palette: 1,2,4,8 bit*/ + LCT_GREY = 0, /*grayscale: 1,2,4,8,16 bit*/ + LCT_RGB = 2, /*RGB: 8,16 bit*/ + LCT_PALETTE = 3, /*palette: 1,2,4,8 bit*/ LCT_GREY_ALPHA = 4, /*grayscale with alpha: 8,16 bit*/ - LCT_RGBA = 6, /*RGB with alpha: 8,16 bit*/ - /*LCT_MAX_OCTET_VALUE lets the compiler allow this enum to represent any invalid - byte value from 0 to 255 that could be present in an invalid PNG file header. Do - not use, compare with or set the name LCT_MAX_OCTET_VALUE, instead either use - the valid color type names above, or numeric values like 1 or 7 when checking for - particular disallowed color type byte values, or cast to integer to print it.*/ + LCT_RGBA = 6, /*RGB with alpha: 8,16 bit*/ + /*LCT_MAX_OCTET_VALUE lets the compiler allow this enum to represent any + invalid byte value from 0 to 255 that could be present in an invalid PNG file + header. Do not use, compare with or set the name LCT_MAX_OCTET_VALUE, instead + either use the valid color type names above, or numeric values like 1 or 7 + when checking for particular disallowed color type byte values, or cast to + integer to print it.*/ LCT_MAX_OCTET_VALUE = 255 } LodePNGColorType; @@ -144,21 +154,22 @@ w: Output parameter. Pointer to width of pixel data. h: Output parameter. Pointer to height of pixel data. in: Memory buffer with the PNG file. insize: size of the in buffer. -colortype: the desired color type for the raw output image. See explanation on PNG color types. -bitdepth: the desired bit depth for the raw output image. See explanation on PNG color types. -Return value: LodePNG error code (0 means no error). +colortype: the desired color type for the raw output image. See explanation on +PNG color types. bitdepth: the desired bit depth for the raw output image. See +explanation on PNG color types. Return value: LodePNG error code (0 means no +error). */ -unsigned lodepng_decode_memory(unsigned char** out, unsigned* w, unsigned* h, - const unsigned char* in, size_t insize, +unsigned lodepng_decode_memory(unsigned char **out, unsigned *w, unsigned *h, + const unsigned char *in, size_t insize, LodePNGColorType colortype, unsigned bitdepth); /*Same as lodepng_decode_memory, but always decodes to 32-bit RGBA raw image*/ -unsigned lodepng_decode32(unsigned char** out, unsigned* w, unsigned* h, - const unsigned char* in, size_t insize); +unsigned lodepng_decode32(unsigned char **out, unsigned *w, unsigned *h, + const unsigned char *in, size_t insize); /*Same as lodepng_decode_memory, but always decodes to 24-bit RGB raw image*/ -unsigned lodepng_decode24(unsigned char** out, unsigned* w, unsigned* h, - const unsigned char* in, size_t insize); +unsigned lodepng_decode24(unsigned char **out, unsigned *w, unsigned *h, + const unsigned char *in, size_t insize); #ifdef LODEPNG_COMPILE_DISK /* @@ -167,27 +178,26 @@ Same as the other decode functions, but instead takes a filename as input. NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and decode in-memory.*/ -unsigned lodepng_decode_file(unsigned char** out, unsigned* w, unsigned* h, - const char* filename, - LodePNGColorType colortype, unsigned bitdepth); +unsigned lodepng_decode_file(unsigned char **out, unsigned *w, unsigned *h, + const char *filename, LodePNGColorType colortype, + unsigned bitdepth); /*Same as lodepng_decode_file, but always decodes to 32-bit RGBA raw image. NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and decode in-memory.*/ -unsigned lodepng_decode32_file(unsigned char** out, unsigned* w, unsigned* h, - const char* filename); +unsigned lodepng_decode32_file(unsigned char **out, unsigned *w, unsigned *h, + const char *filename); /*Same as lodepng_decode_file, but always decodes to 24-bit RGB raw image. NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and decode in-memory.*/ -unsigned lodepng_decode24_file(unsigned char** out, unsigned* w, unsigned* h, - const char* filename); +unsigned lodepng_decode24_file(unsigned char **out, unsigned *w, unsigned *h, + const char *filename); #endif /*LODEPNG_COMPILE_DISK*/ #endif /*LODEPNG_COMPILE_DECODER*/ - #ifdef LODEPNG_COMPILE_ENCODER /* Converts raw pixel data into a PNG image in memory. The colortype and bitdepth @@ -198,24 +208,26 @@ out: Output parameter. Pointer to buffer that will contain the PNG image data. Must be freed after usage with free(*out). outsize: Output parameter. Pointer to the size in bytes of the out buffer. image: The raw pixel data to encode. The size of this buffer should be - w * h * (bytes per pixel), bytes per pixel depends on colortype and bitdepth. -w: width of the raw pixel data in pixels. -h: height of the raw pixel data in pixels. -colortype: the color type of the raw input image. See explanation on PNG color types. -bitdepth: the bit depth of the raw input image. See explanation on PNG color types. -Return value: LodePNG error code (0 means no error). + w * h * (bytes per pixel), bytes per pixel depends on colortype and +bitdepth. w: width of the raw pixel data in pixels. h: height of the raw pixel +data in pixels. colortype: the color type of the raw input image. See +explanation on PNG color types. bitdepth: the bit depth of the raw input image. +See explanation on PNG color types. Return value: LodePNG error code (0 means no +error). */ -unsigned lodepng_encode_memory(unsigned char** out, size_t* outsize, - const unsigned char* image, unsigned w, unsigned h, - LodePNGColorType colortype, unsigned bitdepth); +unsigned lodepng_encode_memory(unsigned char **out, size_t *outsize, + const unsigned char *image, unsigned w, + unsigned h, LodePNGColorType colortype, + unsigned bitdepth); -/*Same as lodepng_encode_memory, but always encodes from 32-bit RGBA raw image.*/ -unsigned lodepng_encode32(unsigned char** out, size_t* outsize, - const unsigned char* image, unsigned w, unsigned h); +/*Same as lodepng_encode_memory, but always encodes from 32-bit RGBA raw + * image.*/ +unsigned lodepng_encode32(unsigned char **out, size_t *outsize, + const unsigned char *image, unsigned w, unsigned h); /*Same as lodepng_encode_memory, but always encodes from 24-bit RGB raw image.*/ -unsigned lodepng_encode24(unsigned char** out, size_t* outsize, - const unsigned char* image, unsigned w, unsigned h); +unsigned lodepng_encode24(unsigned char **out, size_t *outsize, + const unsigned char *image, unsigned w, unsigned h); #ifdef LODEPNG_COMPILE_DISK /* @@ -226,37 +238,36 @@ NOTE: This overwrites existing files without warning! NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and encode in-memory.*/ -unsigned lodepng_encode_file(const char* filename, - const unsigned char* image, unsigned w, unsigned h, - LodePNGColorType colortype, unsigned bitdepth); +unsigned lodepng_encode_file(const char *filename, const unsigned char *image, + unsigned w, unsigned h, LodePNGColorType colortype, + unsigned bitdepth); /*Same as lodepng_encode_file, but always encodes from 32-bit RGBA raw image. NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and encode in-memory.*/ -unsigned lodepng_encode32_file(const char* filename, - const unsigned char* image, unsigned w, unsigned h); +unsigned lodepng_encode32_file(const char *filename, const unsigned char *image, + unsigned w, unsigned h); /*Same as lodepng_encode_file, but always encodes from 24-bit RGB raw image. NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and encode in-memory.*/ -unsigned lodepng_encode24_file(const char* filename, - const unsigned char* image, unsigned w, unsigned h); +unsigned lodepng_encode24_file(const char *filename, const unsigned char *image, + unsigned w, unsigned h); #endif /*LODEPNG_COMPILE_DISK*/ #endif /*LODEPNG_COMPILE_ENCODER*/ - #ifdef LODEPNG_COMPILE_CPP namespace lodepng { #ifdef LODEPNG_COMPILE_DECODER /*Same as lodepng_decode_memory, but decodes to an std::vector. The colortype is the format to output the pixels to. Default is RGBA 8-bit per channel.*/ -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - const unsigned char* in, size_t insize, +unsigned decode(std::vector &out, unsigned &w, unsigned &h, + const unsigned char *in, size_t insize, LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - const std::vector& in, +unsigned decode(std::vector &out, unsigned &w, unsigned &h, + const std::vector &in, LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); #ifdef LODEPNG_COMPILE_DISK /* @@ -266,8 +277,8 @@ Same as the other decode functions, but instead takes a filename as input. NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and decode in-memory. */ -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - const std::string& filename, +unsigned decode(std::vector &out, unsigned &w, unsigned &h, + const std::string &filename, LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); #endif /* LODEPNG_COMPILE_DISK */ #endif /* LODEPNG_COMPILE_DECODER */ @@ -275,11 +286,11 @@ unsigned decode(std::vector& out, unsigned& w, unsigned& h, #ifdef LODEPNG_COMPILE_ENCODER /*Same as lodepng_encode_memory, but encodes to an std::vector. colortype is that of the raw input data. The output PNG color type will be auto chosen.*/ -unsigned encode(std::vector& out, - const unsigned char* in, unsigned w, unsigned h, - LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -unsigned encode(std::vector& out, - const std::vector& in, unsigned w, unsigned h, +unsigned encode(std::vector &out, const unsigned char *in, + unsigned w, unsigned h, LodePNGColorType colortype = LCT_RGBA, + unsigned bitdepth = 8); +unsigned encode(std::vector &out, + const std::vector &in, unsigned w, unsigned h, LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); #ifdef LODEPNG_COMPILE_DISK /* @@ -291,11 +302,11 @@ NOTE: This overwrites existing files without warning! NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and decode in-memory. */ -unsigned encode(const std::string& filename, - const unsigned char* in, unsigned w, unsigned h, - LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -unsigned encode(const std::string& filename, - const std::vector& in, unsigned w, unsigned h, +unsigned encode(const std::string &filename, const unsigned char *in, + unsigned w, unsigned h, LodePNGColorType colortype = LCT_RGBA, + unsigned bitdepth = 8); +unsigned encode(const std::string &filename, + const std::vector &in, unsigned w, unsigned h, LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); #endif /* LODEPNG_COMPILE_DISK */ #endif /* LODEPNG_COMPILE_ENCODER */ @@ -305,41 +316,44 @@ unsigned encode(const std::string& filename, #ifdef LODEPNG_COMPILE_ERROR_TEXT /*Returns an English description of the numerical error code.*/ -const char* lodepng_error_text(unsigned code); +const char *lodepng_error_text(unsigned code); #endif /*LODEPNG_COMPILE_ERROR_TEXT*/ #ifdef LODEPNG_COMPILE_DECODER /*Settings for zlib decompression*/ typedef struct LodePNGDecompressSettings LodePNGDecompressSettings; struct LodePNGDecompressSettings { - /* Check LodePNGDecoderSettings for more ignorable errors such as ignore_crc */ - unsigned ignore_adler32; /*if 1, continue and don't give an error message if the Adler32 checksum is corrupted*/ - unsigned ignore_nlen; /*ignore complement of len checksum in uncompressed blocks*/ - - /*Maximum decompressed size, beyond this the decoder may (and is encouraged to) stop decoding, - return an error, output a data size > max_output_size and all the data up to that point. This is - not hard limit nor a guarantee, but can prevent excessive memory usage. This setting is - ignored by the PNG decoder, but is used by the deflate/zlib decoder and can be used by custom ones. - Set to 0 to impose no limit (the default).*/ + /* Check LodePNGDecoderSettings for more ignorable errors such as ignore_crc + */ + unsigned ignore_adler32; /*if 1, continue and don't give an error message if + the Adler32 checksum is corrupted*/ + unsigned + ignore_nlen; /*ignore complement of len checksum in uncompressed blocks*/ + + /*Maximum decompressed size, beyond this the decoder may (and is encouraged + to) stop decoding, return an error, output a data size > max_output_size and + all the data up to that point. This is not hard limit nor a guarantee, but can + prevent excessive memory usage. This setting is ignored by the PNG decoder, + but is used by the deflate/zlib decoder and can be used by custom ones. Set to + 0 to impose no limit (the default).*/ size_t max_output_size; /*use custom zlib decoder instead of built in one (default: null). Should return 0 if success, any non-0 if error (numeric value not exposed).*/ - unsigned (*custom_zlib)(unsigned char**, size_t*, - const unsigned char*, size_t, - const LodePNGDecompressSettings*); + unsigned (*custom_zlib)(unsigned char **, size_t *, const unsigned char *, + size_t, const LodePNGDecompressSettings *); /*use custom deflate decoder instead of built in one (default: null) - if custom_zlib is not null, custom_inflate is ignored (the zlib format uses deflate). - Should return 0 if success, any non-0 if error (numeric value not exposed).*/ - unsigned (*custom_inflate)(unsigned char**, size_t*, - const unsigned char*, size_t, - const LodePNGDecompressSettings*); + if custom_zlib is not null, custom_inflate is ignored (the zlib format uses + deflate). Should return 0 if success, any non-0 if error (numeric value not + exposed).*/ + unsigned (*custom_inflate)(unsigned char **, size_t *, const unsigned char *, + size_t, const LodePNGDecompressSettings *); - const void* custom_context; /*optional custom settings for custom functions*/ + const void *custom_context; /*optional custom settings for custom functions*/ }; extern const LodePNGDecompressSettings lodepng_default_decompress_settings; -void lodepng_decompress_settings_init(LodePNGDecompressSettings* settings); +void lodepng_decompress_settings_init(LodePNGDecompressSettings *settings); #endif /*LODEPNG_COMPILE_DECODER*/ #ifdef LODEPNG_COMPILE_ENCODER @@ -350,29 +364,33 @@ between speed and compression ratio. typedef struct LodePNGCompressSettings LodePNGCompressSettings; struct LodePNGCompressSettings /*deflate = compress*/ { /*LZ77 related settings*/ - unsigned btype; /*the block type for LZ (0, 1, 2 or 3, see zlib standard). Should be 2 for proper compression.*/ - unsigned use_lz77; /*whether or not to use LZ77. Should be 1 for proper compression.*/ - unsigned windowsize; /*must be a power of two <= 32768. higher compresses more but is slower. Default value: 2048.*/ - unsigned minmatch; /*minimum lz77 length. 3 is normally best, 6 can be better for some PNGs. Default: 0*/ - unsigned nicematch; /*stop searching if >= this length found. Set to 258 for best compression. Default: 128*/ - unsigned lazymatching; /*use lazy matching: better compression but a bit slower. Default: true*/ + unsigned btype; /*the block type for LZ (0, 1, 2 or 3, see zlib standard). + Should be 2 for proper compression.*/ + unsigned use_lz77; /*whether or not to use LZ77. Should be 1 for proper + compression.*/ + unsigned windowsize; /*must be a power of two <= 32768. higher compresses more + but is slower. Default value: 2048.*/ + unsigned minmatch; /*minimum lz77 length. 3 is normally best, 6 can be better + for some PNGs. Default: 0*/ + unsigned nicematch; /*stop searching if >= this length found. Set to 258 for + best compression. Default: 128*/ + unsigned lazymatching; /*use lazy matching: better compression but a bit + slower. Default: true*/ /*use custom zlib encoder instead of built in one (default: null)*/ - unsigned (*custom_zlib)(unsigned char**, size_t*, - const unsigned char*, size_t, - const LodePNGCompressSettings*); + unsigned (*custom_zlib)(unsigned char **, size_t *, const unsigned char *, + size_t, const LodePNGCompressSettings *); /*use custom deflate encoder instead of built in one (default: null) if custom_zlib is used, custom_deflate is ignored since only the built in zlib function will call custom_deflate*/ - unsigned (*custom_deflate)(unsigned char**, size_t*, - const unsigned char*, size_t, - const LodePNGCompressSettings*); + unsigned (*custom_deflate)(unsigned char **, size_t *, const unsigned char *, + size_t, const LodePNGCompressSettings *); - const void* custom_context; /*optional custom settings for custom functions*/ + const void *custom_context; /*optional custom settings for custom functions*/ }; extern const LodePNGCompressSettings lodepng_default_compress_settings; -void lodepng_compress_settings_init(LodePNGCompressSettings* settings); +void lodepng_compress_settings_init(LodePNGCompressSettings *settings); #endif /*LODEPNG_COMPILE_ENCODER*/ #ifdef LODEPNG_COMPILE_PNG @@ -383,8 +401,10 @@ format, and is used both for PNG and raw image data in LodePNG. */ typedef struct LodePNGColorMode { /*header (IHDR)*/ - LodePNGColorType colortype; /*color type, see PNG standard or documentation further in this header file*/ - unsigned bitdepth; /*bits per sample, see PNG standard or documentation further in this header file*/ + LodePNGColorType colortype; /*color type, see PNG standard or documentation + further in this header file*/ + unsigned bitdepth; /*bits per sample, see PNG standard or documentation + further in this header file*/ /* palette (PLTE and tRNS) @@ -403,114 +423,131 @@ typedef struct LodePNGColorMode { The palette is only supported for color type 3. */ - unsigned char* palette; /*palette in RGBARGBA... order. Must be either 0, or when allocated must have 1024 bytes*/ - size_t palettesize; /*palette size in number of colors (amount of used bytes is 4 * palettesize)*/ + unsigned char *palette; /*palette in RGBARGBA... order. Must be either 0, or + when allocated must have 1024 bytes*/ + size_t palettesize; /*palette size in number of colors (amount of used bytes + is 4 * palettesize)*/ /* transparent color key (tRNS) - This color uses the same bit depth as the bitdepth value in this struct, which can be 1-bit to 16-bit. - For grayscale PNGs, r, g and b will all 3 be set to the same. + This color uses the same bit depth as the bitdepth value in this struct, which + can be 1-bit to 16-bit. For grayscale PNGs, r, g and b will all 3 be set to + the same. When decoding, by default you can ignore this information, since LodePNG sets pixels with this key to transparent already in the raw RGBA output. The color key is only supported for color types 0 and 2. */ - unsigned key_defined; /*is a transparent color key given? 0 = false, 1 = true*/ - unsigned key_r; /*red/grayscale component of color key*/ - unsigned key_g; /*green component of color key*/ - unsigned key_b; /*blue component of color key*/ + unsigned + key_defined; /*is a transparent color key given? 0 = false, 1 = true*/ + unsigned key_r; /*red/grayscale component of color key*/ + unsigned key_g; /*green component of color key*/ + unsigned key_b; /*blue component of color key*/ } LodePNGColorMode; /*init, cleanup and copy functions to use with this struct*/ -void lodepng_color_mode_init(LodePNGColorMode* info); -void lodepng_color_mode_cleanup(LodePNGColorMode* info); +void lodepng_color_mode_init(LodePNGColorMode *info); +void lodepng_color_mode_cleanup(LodePNGColorMode *info); /*return value is error code (0 means no error)*/ -unsigned lodepng_color_mode_copy(LodePNGColorMode* dest, const LodePNGColorMode* source); +unsigned lodepng_color_mode_copy(LodePNGColorMode *dest, + const LodePNGColorMode *source); /* Makes a temporary LodePNGColorMode that does not need cleanup (no palette) */ -LodePNGColorMode lodepng_color_mode_make(LodePNGColorType colortype, unsigned bitdepth); +LodePNGColorMode lodepng_color_mode_make(LodePNGColorType colortype, + unsigned bitdepth); -void lodepng_palette_clear(LodePNGColorMode* info); +void lodepng_palette_clear(LodePNGColorMode *info); /*add 1 color to the palette*/ -unsigned lodepng_palette_add(LodePNGColorMode* info, - unsigned char r, unsigned char g, unsigned char b, unsigned char a); +unsigned lodepng_palette_add(LodePNGColorMode *info, unsigned char r, + unsigned char g, unsigned char b, unsigned char a); -/*get the total amount of bits per pixel, based on colortype and bitdepth in the struct*/ -unsigned lodepng_get_bpp(const LodePNGColorMode* info); +/*get the total amount of bits per pixel, based on colortype and bitdepth in the + * struct*/ +unsigned lodepng_get_bpp(const LodePNGColorMode *info); /*get the amount of color channels used, based on colortype in the struct. If a palette is used, it counts as 1 channel.*/ -unsigned lodepng_get_channels(const LodePNGColorMode* info); +unsigned lodepng_get_channels(const LodePNGColorMode *info); /*is it a grayscale type? (only colortype 0 or 4)*/ -unsigned lodepng_is_greyscale_type(const LodePNGColorMode* info); +unsigned lodepng_is_greyscale_type(const LodePNGColorMode *info); /*has it got an alpha channel? (only colortype 2 or 6)*/ -unsigned lodepng_is_alpha_type(const LodePNGColorMode* info); +unsigned lodepng_is_alpha_type(const LodePNGColorMode *info); /*has it got a palette? (only colortype 3)*/ -unsigned lodepng_is_palette_type(const LodePNGColorMode* info); -/*only returns true if there is a palette and there is a value in the palette with alpha < 255. -Loops through the palette to check this.*/ -unsigned lodepng_has_palette_alpha(const LodePNGColorMode* info); +unsigned lodepng_is_palette_type(const LodePNGColorMode *info); +/*only returns true if there is a palette and there is a value in the palette +with alpha < 255. Loops through the palette to check this.*/ +unsigned lodepng_has_palette_alpha(const LodePNGColorMode *info); /* -Check if the given color info indicates the possibility of having non-opaque pixels in the PNG image. -Returns true if the image can have translucent or invisible pixels (it still be opaque if it doesn't use such pixels). -Returns false if the image can only have opaque pixels. -In detail, it returns true only if it's a color type with alpha, or has a palette with non-opaque values, -or if "key_defined" is true. +Check if the given color info indicates the possibility of having non-opaque +pixels in the PNG image. Returns true if the image can have translucent or +invisible pixels (it still be opaque if it doesn't use such pixels). Returns +false if the image can only have opaque pixels. In detail, it returns true only +if it's a color type with alpha, or has a palette with non-opaque values, or if +"key_defined" is true. */ -unsigned lodepng_can_have_alpha(const LodePNGColorMode* info); -/*Returns the byte size of a raw image buffer with given width, height and color mode*/ -size_t lodepng_get_raw_size(unsigned w, unsigned h, const LodePNGColorMode* color); +unsigned lodepng_can_have_alpha(const LodePNGColorMode *info); +/*Returns the byte size of a raw image buffer with given width, height and color + * mode*/ +size_t lodepng_get_raw_size(unsigned w, unsigned h, + const LodePNGColorMode *color); #ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS /*The information of a Time chunk in PNG.*/ typedef struct LodePNGTime { - unsigned year; /*2 bytes used (0-65535)*/ - unsigned month; /*1-12*/ - unsigned day; /*1-31*/ - unsigned hour; /*0-23*/ - unsigned minute; /*0-59*/ - unsigned second; /*0-60 (to allow for leap seconds)*/ + unsigned year; /*2 bytes used (0-65535)*/ + unsigned month; /*1-12*/ + unsigned day; /*1-31*/ + unsigned hour; /*0-23*/ + unsigned minute; /*0-59*/ + unsigned second; /*0-60 (to allow for leap seconds)*/ } LodePNGTime; #endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ /*Information about the PNG image, except pixels, width and height.*/ typedef struct LodePNGInfo { /*header (IHDR), palette (PLTE) and transparency (tRNS) chunks*/ - unsigned compression_method;/*compression method of the original file. Always 0.*/ - unsigned filter_method; /*filter method of the original file*/ - unsigned interlace_method; /*interlace method of the original file: 0=none, 1=Adam7*/ - LodePNGColorMode color; /*color type and bits, palette and transparency of the PNG file*/ + unsigned + compression_method; /*compression method of the original file. Always 0.*/ + unsigned filter_method; /*filter method of the original file*/ + unsigned interlace_method; /*interlace method of the original file: 0=none, + 1=Adam7*/ + LodePNGColorMode + color; /*color type and bits, palette and transparency of the PNG file*/ #ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS /* Suggested background color chunk (bKGD) - This uses the same color mode and bit depth as the PNG (except no alpha channel), - with values truncated to the bit depth in the unsigned integer. + This uses the same color mode and bit depth as the PNG (except no alpha + channel), with values truncated to the bit depth in the unsigned integer. - For grayscale and palette PNGs, the value is stored in background_r. The values - in background_g and background_b are then unused. The decoder will set them - equal to background_r, the encoder ignores them in this case. + For grayscale and palette PNGs, the value is stored in background_r. The + values in background_g and background_b are then unused. The decoder will set + them equal to background_r, the encoder ignores them in this case. - When decoding, you may get these in a different color mode than the one you requested - for the raw pixels: the colortype and bitdepth defined by info_png.color, that is the - ones defined in the header of the PNG image, are used. + When decoding, you may get these in a different color mode than the one you + requested for the raw pixels: the colortype and bitdepth defined by + info_png.color, that is the ones defined in the header of the PNG image, are + used. - When encoding with auto_convert, you must use the color model defined in info_png.color for - these values. The encoder normally ignores info_png.color when auto_convert is on, but will - use it to interpret these values (and convert copies of them to its chosen color model). + When encoding with auto_convert, you must use the color model defined in + info_png.color for these values. The encoder normally ignores info_png.color + when auto_convert is on, but will use it to interpret these values (and + convert copies of them to its chosen color model). - When encoding, avoid setting this to an expensive color, such as a non-gray value - when the image is gray, or the compression will be worse since it will be forced to - write the PNG with a more expensive color mode (when auto_convert is on). + When encoding, avoid setting this to an expensive color, such as a non-gray + value when the image is gray, or the compression will be worse since it will + be forced to write the PNG with a more expensive color mode (when auto_convert + is on). - The decoder does not use this background color to edit the color of pixels. This is a - completely optional metadata feature. + The decoder does not use this background color to edit the color of pixels. + This is a completely optional metadata feature. */ unsigned background_defined; /*is a suggested background color given?*/ - unsigned background_r; /*red/gray/palette component of suggested background color*/ - unsigned background_g; /*green component of suggested background color*/ - unsigned background_b; /*blue component of suggested background color*/ + unsigned + background_r; /*red/gray/palette component of suggested background color*/ + unsigned background_g; /*green component of suggested background color*/ + unsigned background_b; /*blue component of suggested background color*/ /* Non-international text chunks (tEXt and zTXt) @@ -519,9 +556,10 @@ typedef struct LodePNGInfo { text_strings, while text_keys are keywords that give a short description what the actual text represents, e.g. Title, Author, Description, or anything else. - All the string fields below including strings, keys, names and language tags are null terminated. - The PNG specification uses null characters for the keys, names and tags, and forbids null - characters to appear in the main text which is why we can use null termination everywhere here. + All the string fields below including strings, keys, names and language tags + are null terminated. The PNG specification uses null characters for the keys, + names and tags, and forbids null characters to appear in the main text which + is why we can use null termination everywhere here. A keyword is minimum 1 character and maximum 79 characters long (plus the additional null terminator). It's discouraged to use a single line length @@ -532,9 +570,10 @@ typedef struct LodePNGInfo { Standard text chunk keywords and strings are encoded using Latin-1. */ - size_t text_num; /*the amount of texts in these char** buffers (there may be more texts in itext)*/ - char** text_keys; /*the keyword of a text chunk (e.g. "Comment")*/ - char** text_strings; /*the actual text*/ + size_t text_num; /*the amount of texts in these char** buffers (there may be + more texts in itext)*/ + char **text_keys; /*the keyword of a text chunk (e.g. "Comment")*/ + char **text_strings; /*the actual text*/ /* International text chunks (iTXt) @@ -544,38 +583,45 @@ typedef struct LodePNGInfo { keys must be 1-79 characters (plus the additional null terminator), the other strings are any length. */ - size_t itext_num; /*the amount of international texts in this PNG*/ - char** itext_keys; /*the English keyword of the text chunk (e.g. "Comment")*/ - char** itext_langtags; /*language tag for this text's language, ISO/IEC 646 string, e.g. ISO 639 language tag*/ - char** itext_transkeys; /*keyword translated to the international language - UTF-8 string*/ - char** itext_strings; /*the actual international text - UTF-8 string*/ + size_t itext_num; /*the amount of international texts in this PNG*/ + char **itext_keys; /*the English keyword of the text chunk (e.g. "Comment")*/ + char **itext_langtags; /*language tag for this text's language, ISO/IEC 646 + string, e.g. ISO 639 language tag*/ + char **itext_transkeys; /*keyword translated to the international language - + UTF-8 string*/ + char **itext_strings; /*the actual international text - UTF-8 string*/ /*time chunk (tIME)*/ unsigned time_defined; /*set to 1 to make the encoder generate a tIME chunk*/ LodePNGTime time; /*phys chunk (pHYs)*/ - unsigned phys_defined; /*if 0, there is no pHYs chunk and the values below are undefined, if 1 else there is one*/ - unsigned phys_x; /*pixels per unit in x direction*/ - unsigned phys_y; /*pixels per unit in y direction*/ - unsigned phys_unit; /*may be 0 (unknown unit) or 1 (metre)*/ + unsigned phys_defined; /*if 0, there is no pHYs chunk and the values below are + undefined, if 1 else there is one*/ + unsigned phys_x; /*pixels per unit in x direction*/ + unsigned phys_y; /*pixels per unit in y direction*/ + unsigned phys_unit; /*may be 0 (unknown unit) or 1 (metre)*/ /* Color profile related chunks: gAMA, cHRM, sRGB, iCPP, sBIT - LodePNG does not apply any color conversions on pixels in the encoder or decoder and does not interpret these color - profile values. It merely passes on the information. If you wish to use color profiles and convert colors, please + LodePNG does not apply any color conversions on pixels in the encoder or + decoder and does not interpret these color profile values. It merely passes on + the information. If you wish to use color profiles and convert colors, please use these values with a color management library. - See the PNG, ICC and sRGB specifications for more information about the meaning of these values. + See the PNG, ICC and sRGB specifications for more information about the + meaning of these values. */ /* gAMA chunk: optional, overridden by sRGB or iCCP if those are present. */ - unsigned gama_defined; /* Whether a gAMA chunk is present (0 = not present, 1 = present). */ + unsigned gama_defined; /* Whether a gAMA chunk is present (0 = not present, 1 + = present). */ unsigned gama_gamma; /* Gamma exponent times 100000 */ /* cHRM chunk: optional, overridden by sRGB or iCCP if those are present. */ - unsigned chrm_defined; /* Whether a cHRM chunk is present (0 = not present, 1 = present). */ + unsigned chrm_defined; /* Whether a cHRM chunk is present (0 = not present, 1 + = present). */ unsigned chrm_white_x; /* White Point x times 100000 */ unsigned chrm_white_y; /* White Point y times 100000 */ unsigned chrm_red_x; /* Red x times 100000 */ @@ -588,76 +634,94 @@ typedef struct LodePNGInfo { /* sRGB chunk: optional. May not appear at the same time as iCCP. If gAMA is also present gAMA must contain value 45455. - If cHRM is also present cHRM must contain respectively 31270,32900,64000,33000,30000,60000,15000,6000. + If cHRM is also present cHRM must contain respectively + 31270,32900,64000,33000,30000,60000,15000,6000. */ - unsigned srgb_defined; /* Whether an sRGB chunk is present (0 = not present, 1 = present). */ - unsigned srgb_intent; /* Rendering intent: 0=perceptual, 1=rel. colorimetric, 2=saturation, 3=abs. colorimetric */ + unsigned srgb_defined; /* Whether an sRGB chunk is present (0 = not present, 1 + = present). */ + unsigned srgb_intent; /* Rendering intent: 0=perceptual, 1=rel. colorimetric, + 2=saturation, 3=abs. colorimetric */ /* iCCP chunk: optional. May not appear at the same time as sRGB. - LodePNG does not parse or use the ICC profile (except its color space header field for an edge case), a - separate library to handle the ICC data (not included in LodePNG) format is needed to use it for color - management and conversions. - - For encoding, if iCCP is present, gAMA and cHRM are recommended to be added as well with values that match the ICC - profile as closely as possible, if you wish to do this you should provide the correct values for gAMA and cHRM and - enable their '_defined' flags since LodePNG will not automatically compute them from the ICC profile. - - For encoding, the ICC profile is required by the PNG specification to be an "RGB" profile for non-gray - PNG color types and a "GRAY" profile for gray PNG color types. If you disable auto_convert, you must ensure - the ICC profile type matches your requested color type, else the encoder gives an error. If auto_convert is - enabled (the default), and the ICC profile is not a good match for the pixel data, this will result in an encoder - error if the pixel data has non-gray pixels for a GRAY profile, or a silent less-optimal compression of the pixel - data if the pixels could be encoded as grayscale but the ICC profile is RGB. - - To avoid this do not set an ICC profile in the image unless there is a good reason for it, and when doing so - make sure you compute it carefully to avoid the above problems. + LodePNG does not parse or use the ICC profile (except its color space header + field for an edge case), a separate library to handle the ICC data (not + included in LodePNG) format is needed to use it for color management and + conversions. + + For encoding, if iCCP is present, gAMA and cHRM are recommended to be added as + well with values that match the ICC profile as closely as possible, if you + wish to do this you should provide the correct values for gAMA and cHRM and + enable their '_defined' flags since LodePNG will not automatically compute + them from the ICC profile. + + For encoding, the ICC profile is required by the PNG specification to be an + "RGB" profile for non-gray PNG color types and a "GRAY" profile for gray PNG + color types. If you disable auto_convert, you must ensure the ICC profile type + matches your requested color type, else the encoder gives an error. If + auto_convert is enabled (the default), and the ICC profile is not a good match + for the pixel data, this will result in an encoder error if the pixel data has + non-gray pixels for a GRAY profile, or a silent less-optimal compression of + the pixel data if the pixels could be encoded as grayscale but the ICC profile + is RGB. + + To avoid this do not set an ICC profile in the image unless there is a good + reason for it, and when doing so make sure you compute it carefully to avoid + the above problems. */ - unsigned iccp_defined; /* Whether an iCCP chunk is present (0 = not present, 1 = present). */ - char* iccp_name; /* Null terminated string with profile name, 1-79 bytes */ + unsigned iccp_defined; /* Whether an iCCP chunk is present (0 = not present, 1 + = present). */ + char *iccp_name; /* Null terminated string with profile name, 1-79 bytes */ /* The ICC profile in iccp_profile_size bytes. Don't allocate this buffer yourself. Use the init/cleanup functions correctly and use lodepng_set_icc and lodepng_clear_icc. */ - unsigned char* iccp_profile; + unsigned char *iccp_profile; unsigned iccp_profile_size; /* The size of iccp_profile in bytes */ /* sBIT chunk: significant bits. Optional metadata, only set this if needed. - If defined, these values give the bit depth of the original data. Since PNG only stores 1, 2, 4, 8 or 16-bit - per channel data, the significant bits value can be used to indicate the original encoded data has another - sample depth, such as 10 or 12. - - Encoders using this value, when storing the pixel data, should use the most significant bits - of the data to store the original bits, and use a good sample depth scaling method such as - "left bit replication" to fill in the least significant bits, rather than fill zeroes. - - Decoders using this value, if able to work with data that's e.g. 10-bit or 12-bit, should right - shift the data to go back to the original bit depth, but decoders are also allowed to ignore - sbit and work e.g. with the 8-bit or 16-bit data from the PNG directly, since thanks - to the encoder contract, the values encoded in PNG are in valid range for the PNG bit depth. - - For grayscale images, sbit_g and sbit_b are not used, and for images that don't use color - type RGBA or grayscale+alpha, sbit_a is not used (it's not used even for palette images with - translucent palette values, or images with color key). The values that are used must be - greater than zero and smaller than or equal to the PNG bit depth. - - The color type from the header in the PNG image defines these used and unused fields: if - decoding with a color mode conversion, such as always decoding to RGBA, this metadata still - only uses the color type of the original PNG, and may e.g. lack the alpha channel info - if the PNG was RGB. When encoding with auto_convert (as well as without), also always the - color model defined in info_png.color determines this. - - NOTE: enabling sbit can hurt compression, because the encoder can then not always use - auto_convert to choose a more optimal color mode for the data, because the PNG format has - strict requirements for the allowed sbit values in combination with color modes. - For example, setting these fields to 10-bit will force the encoder to keep using a 16-bit per channel - color mode, even if the pixel data would in fact fit in a more efficient 8-bit mode. + If defined, these values give the bit depth of the original data. Since PNG + only stores 1, 2, 4, 8 or 16-bit per channel data, the significant bits value + can be used to indicate the original encoded data has another sample depth, + such as 10 or 12. + + Encoders using this value, when storing the pixel data, should use the most + significant bits of the data to store the original bits, and use a good sample + depth scaling method such as "left bit replication" to fill in the least + significant bits, rather than fill zeroes. + + Decoders using this value, if able to work with data that's e.g. 10-bit or + 12-bit, should right shift the data to go back to the original bit depth, but + decoders are also allowed to ignore sbit and work e.g. with the 8-bit or + 16-bit data from the PNG directly, since thanks to the encoder contract, the + values encoded in PNG are in valid range for the PNG bit depth. + + For grayscale images, sbit_g and sbit_b are not used, and for images that + don't use color type RGBA or grayscale+alpha, sbit_a is not used (it's not + used even for palette images with translucent palette values, or images with + color key). The values that are used must be greater than zero and smaller + than or equal to the PNG bit depth. + + The color type from the header in the PNG image defines these used and unused + fields: if decoding with a color mode conversion, such as always decoding to + RGBA, this metadata still only uses the color type of the original PNG, and + may e.g. lack the alpha channel info if the PNG was RGB. When encoding with + auto_convert (as well as without), also always the color model defined in + info_png.color determines this. + + NOTE: enabling sbit can hurt compression, because the encoder can then not + always use auto_convert to choose a more optimal color mode for the data, + because the PNG format has strict requirements for the allowed sbit values in + combination with color modes. For example, setting these fields to 10-bit will + force the encoder to keep using a 16-bit per channel color mode, even if the + pixel data would in fact fit in a more efficient 8-bit mode. */ - unsigned sbit_defined; /*is significant bits given? if not, the values below are unused*/ + unsigned sbit_defined; /*is significant bits given? if not, the values below + are unused*/ unsigned sbit_r; /*red or gray component of significant bits*/ unsigned sbit_g; /*green component of significant bits*/ unsigned sbit_b; /*blue component of significant bits*/ @@ -665,62 +729,72 @@ typedef struct LodePNGInfo { /* End of color profile related chunks */ - /* unknown chunks: chunks not known by LodePNG, passed on byte for byte. - There are 3 buffers, one for each position in the PNG where unknown chunks can appear. - Each buffer contains all unknown chunks for that position consecutively. - The 3 positions are: - 0: between IHDR and PLTE, 1: between PLTE and IDAT, 2: between IDAT and IEND. - - For encoding, do not store critical chunks or known chunks that are enabled with a "_defined" flag - above in here, since the encoder will blindly follow this and could then encode an invalid PNG file - (such as one with two IHDR chunks or the disallowed combination of sRGB with iCCP). But do use - this if you wish to store an ancillary chunk that is not supported by LodePNG (such as sPLT or hIST), - or any non-standard PNG chunk. - - Do not allocate or traverse this data yourself. Use the chunk traversing functions declared - later, such as lodepng_chunk_next and lodepng_chunk_append, to read/write this struct. + There are 3 buffers, one for each position in the PNG where unknown chunks can + appear. Each buffer contains all unknown chunks for that position + consecutively. The 3 positions are: 0: between IHDR and PLTE, 1: between PLTE + and IDAT, 2: between IDAT and IEND. + + For encoding, do not store critical chunks or known chunks that are enabled + with a "_defined" flag above in here, since the encoder will blindly follow + this and could then encode an invalid PNG file (such as one with two IHDR + chunks or the disallowed combination of sRGB with iCCP). But do use this if + you wish to store an ancillary chunk that is not supported by LodePNG (such as + sPLT or hIST), or any non-standard PNG chunk. + + Do not allocate or traverse this data yourself. Use the chunk traversing + functions declared later, such as lodepng_chunk_next and lodepng_chunk_append, + to read/write this struct. */ - unsigned char* unknown_chunks_data[3]; - size_t unknown_chunks_size[3]; /*size in bytes of the unknown chunks, given for protection*/ -#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ + unsigned char *unknown_chunks_data[3]; + size_t unknown_chunks_size[3]; /*size in bytes of the unknown chunks, given + for protection*/ +#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ } LodePNGInfo; /*init, cleanup and copy functions to use with this struct*/ -void lodepng_info_init(LodePNGInfo* info); -void lodepng_info_cleanup(LodePNGInfo* info); +void lodepng_info_init(LodePNGInfo *info); +void lodepng_info_cleanup(LodePNGInfo *info); /*return value is error code (0 means no error)*/ -unsigned lodepng_info_copy(LodePNGInfo* dest, const LodePNGInfo* source); +unsigned lodepng_info_copy(LodePNGInfo *dest, const LodePNGInfo *source); #ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS -unsigned lodepng_add_text(LodePNGInfo* info, const char* key, const char* str); /*push back both texts at once*/ -void lodepng_clear_text(LodePNGInfo* info); /*use this to clear the texts again after you filled them in*/ - -unsigned lodepng_add_itext(LodePNGInfo* info, const char* key, const char* langtag, - const char* transkey, const char* str); /*push back the 4 texts of 1 chunk at once*/ -void lodepng_clear_itext(LodePNGInfo* info); /*use this to clear the itexts again after you filled them in*/ +unsigned lodepng_add_text(LodePNGInfo *info, const char *key, + const char *str); /*push back both texts at once*/ +void lodepng_clear_text(LodePNGInfo *info); /*use this to clear the texts again + after you filled them in*/ + +unsigned +lodepng_add_itext(LodePNGInfo *info, const char *key, const char *langtag, + const char *transkey, + const char *str); /*push back the 4 texts of 1 chunk at once*/ +void lodepng_clear_itext(LodePNGInfo *info); /*use this to clear the itexts + again after you filled them in*/ /*replaces if exists*/ -unsigned lodepng_set_icc(LodePNGInfo* info, const char* name, const unsigned char* profile, unsigned profile_size); -void lodepng_clear_icc(LodePNGInfo* info); /*use this to clear the texts again after you filled them in*/ -#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ +unsigned lodepng_set_icc(LodePNGInfo *info, const char *name, + const unsigned char *profile, unsigned profile_size); +void lodepng_clear_icc(LodePNGInfo *info); /*use this to clear the texts again + after you filled them in*/ +#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ /* Converts raw buffer from one color type to another color type, based on LodePNGColorMode structs to describe the input and output color type. -See the reference manual at the end of this header file to see which color conversions are supported. -return value = LodePNG error code (0 if all went ok, an error if the conversion isn't supported) -The out buffer must have size (w * h * bpp + 7) / 8, where bpp is the bits per pixel -of the output color type (lodepng_get_bpp). -For < 8 bpp images, there should not be padding bits at the end of scanlines. -For 16-bit per channel colors, uses big endian format like PNG does. -Return value is LodePNG error code +See the reference manual at the end of this header file to see which color +conversions are supported. return value = LodePNG error code (0 if all went ok, +an error if the conversion isn't supported) The out buffer must have size (w * h +* bpp + 7) / 8, where bpp is the bits per pixel of the output color type +(lodepng_get_bpp). For < 8 bpp images, there should not be padding bits at the +end of scanlines. For 16-bit per channel colors, uses big endian format like PNG +does. Return value is LodePNG error code */ -unsigned lodepng_convert(unsigned char* out, const unsigned char* in, - const LodePNGColorMode* mode_out, const LodePNGColorMode* mode_in, - unsigned w, unsigned h); +unsigned lodepng_convert(unsigned char *out, const unsigned char *in, + const LodePNGColorMode *mode_out, + const LodePNGColorMode *mode_in, unsigned w, + unsigned h); #ifdef LODEPNG_COMPILE_DECODER /* @@ -728,190 +802,220 @@ Settings for the decoder. This contains settings for the PNG and the Zlib decoder, but not the Info settings from the Info structs. */ typedef struct LodePNGDecoderSettings { - LodePNGDecompressSettings zlibsettings; /*in here is the setting to ignore Adler32 checksums*/ + LodePNGDecompressSettings + zlibsettings; /*in here is the setting to ignore Adler32 checksums*/ - /* Check LodePNGDecompressSettings for more ignorable errors such as ignore_adler32 */ - unsigned ignore_crc; /*ignore CRC checksums*/ + /* Check LodePNGDecompressSettings for more ignorable errors such as + * ignore_adler32 */ + unsigned ignore_crc; /*ignore CRC checksums*/ unsigned ignore_critical; /*ignore unknown critical chunks*/ - unsigned ignore_end; /*ignore issues at end of file if possible (missing IEND chunk, too large chunk, ...)*/ - /* TODO: make a system involving warnings with levels and a strict mode instead. Other potentially recoverable - errors: srgb rendering intent value, size of content of ancillary chunks, more than 79 characters for some - strings, placement/combination rules for ancillary chunks, crc of unknown chunks, allowed characters - in string keys, etc... */ + unsigned ignore_end; /*ignore issues at end of file if possible (missing IEND + chunk, too large chunk, ...)*/ + /* TODO: make a system involving warnings with levels and a strict mode + instead. Other potentially recoverable errors: srgb rendering intent value, + size of content of ancillary chunks, more than 79 characters for some + strings, placement/combination rules for ancillary chunks, crc of unknown + chunks, allowed characters in string keys, etc... */ - unsigned color_convert; /*whether to convert the PNG to the color type you want. Default: yes*/ + unsigned color_convert; /*whether to convert the PNG to the color type you + want. Default: yes*/ #ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS - unsigned read_text_chunks; /*if false but remember_unknown_chunks is true, they're stored in the unknown chunks*/ + unsigned read_text_chunks; /*if false but remember_unknown_chunks is true, + they're stored in the unknown chunks*/ - /*store all bytes from unknown chunks in the LodePNGInfo (off by default, useful for a png editor)*/ + /*store all bytes from unknown chunks in the LodePNGInfo (off by default, + * useful for a png editor)*/ unsigned remember_unknown_chunks; - /* maximum size for decompressed text chunks. If a text chunk's text is larger than this, an error is returned, - unless reading text chunks is disabled or this limit is set higher or disabled. Set to 0 to allow any size. - By default it is a value that prevents unreasonably large strings from hogging memory. */ + /* maximum size for decompressed text chunks. If a text chunk's text is larger + than this, an error is returned, unless reading text chunks is disabled or + this limit is set higher or disabled. Set to 0 to allow any size. By default + it is a value that prevents unreasonably large strings from hogging memory. */ size_t max_text_size; - /* maximum size for compressed ICC chunks. If the ICC profile is larger than this, an error will be returned. Set to - 0 to allow any size. By default this is a value that prevents ICC profiles that would be much larger than any + /* maximum size for compressed ICC chunks. If the ICC profile is larger than + this, an error will be returned. Set to 0 to allow any size. By default this + is a value that prevents ICC profiles that would be much larger than any legitimate profile could be to hog memory. */ size_t max_icc_size; #endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ } LodePNGDecoderSettings; -void lodepng_decoder_settings_init(LodePNGDecoderSettings* settings); +void lodepng_decoder_settings_init(LodePNGDecoderSettings *settings); #endif /*LODEPNG_COMPILE_DECODER*/ #ifdef LODEPNG_COMPILE_ENCODER -/*automatically use color type with less bits per pixel if losslessly possible. Default: AUTO*/ +/*automatically use color type with less bits per pixel if losslessly possible. + * Default: AUTO*/ typedef enum LodePNGFilterStrategy { /*every filter at zero*/ LFS_ZERO = 0, - /*every filter at 1, 2, 3 or 4 (paeth), unlike LFS_ZERO not a good choice, but for testing*/ + /*every filter at 1, 2, 3 or 4 (paeth), unlike LFS_ZERO not a good choice, but + for testing*/ LFS_ONE = 1, LFS_TWO = 2, LFS_THREE = 3, LFS_FOUR = 4, - /*Use filter that gives minimum sum, as described in the official PNG filter heuristic.*/ + /*Use filter that gives minimum sum, as described in the official PNG filter + heuristic.*/ LFS_MINSUM, - /*Use the filter type that gives smallest Shannon entropy for this scanline. Depending - on the image, this is better or worse than minsum.*/ + /*Use the filter type that gives smallest Shannon entropy for this scanline. + Depending on the image, this is better or worse than minsum.*/ LFS_ENTROPY, /* Brute-force-search PNG filters by compressing each filter for each scanline. Experimental, very slow, and only rarely gives better compression than MINSUM. */ LFS_BRUTE_FORCE, - /*use predefined_filters buffer: you specify the filter type for each scanline*/ + /*use predefined_filters buffer: you specify the filter type for each + scanline*/ LFS_PREDEFINED } LodePNGFilterStrategy; -/*Gives characteristics about the integer RGBA colors of the image (count, alpha channel usage, bit depth, ...), -which helps decide which color model to use for encoding. -Used internally by default if "auto_convert" is enabled. Public because it's useful for custom algorithms.*/ +/*Gives characteristics about the integer RGBA colors of the image (count, alpha +channel usage, bit depth, ...), which helps decide which color model to use for +encoding. Used internally by default if "auto_convert" is enabled. Public +because it's useful for custom algorithms.*/ typedef struct LodePNGColorStats { unsigned colored; /*not grayscale*/ - unsigned key; /*image is not opaque and color key is possible instead of full alpha*/ - unsigned short key_r; /*key values, always as 16-bit, in 8-bit case the byte is duplicated, e.g. 65535 means 255*/ + unsigned key; /*image is not opaque and color key is possible instead of full + alpha*/ + unsigned short key_r; /*key values, always as 16-bit, in 8-bit case the byte + is duplicated, e.g. 65535 means 255*/ unsigned short key_g; unsigned short key_b; - unsigned alpha; /*image is not opaque and alpha channel or alpha palette required*/ - unsigned numcolors; /*amount of colors, up to 257. Not valid if bits == 16 or allow_palette is disabled.*/ - unsigned char palette[1024]; /*Remembers up to the first 256 RGBA colors, in no particular order, only valid when numcolors is valid*/ - unsigned bits; /*bits per channel (not for palette). 1,2 or 4 for grayscale only. 16 if 16-bit per channel required.*/ + unsigned + alpha; /*image is not opaque and alpha channel or alpha palette required*/ + unsigned numcolors; /*amount of colors, up to 257. Not valid if bits == 16 or + allow_palette is disabled.*/ + unsigned char + palette[1024]; /*Remembers up to the first 256 RGBA colors, in no + particular order, only valid when numcolors is valid*/ + unsigned bits; /*bits per channel (not for palette). 1,2 or 4 for grayscale + only. 16 if 16-bit per channel required.*/ size_t numpixels; /*user settings for computing/using the stats*/ - unsigned allow_palette; /*default 1. if 0, disallow choosing palette colortype in auto_choose_color, and don't count numcolors*/ - unsigned allow_greyscale; /*default 1. if 0, choose RGB or RGBA even if the image only has gray colors*/ + unsigned allow_palette; /*default 1. if 0, disallow choosing palette colortype + in auto_choose_color, and don't count numcolors*/ + unsigned allow_greyscale; /*default 1. if 0, choose RGB or RGBA even if the + image only has gray colors*/ } LodePNGColorStats; -void lodepng_color_stats_init(LodePNGColorStats* stats); +void lodepng_color_stats_init(LodePNGColorStats *stats); /*Get a LodePNGColorStats of the image. The stats must already have been inited. Returns error code (e.g. alloc fail) or 0 if ok.*/ -unsigned lodepng_compute_color_stats(LodePNGColorStats* stats, - const unsigned char* image, unsigned w, unsigned h, - const LodePNGColorMode* mode_in); +unsigned lodepng_compute_color_stats(LodePNGColorStats *stats, + const unsigned char *image, unsigned w, + unsigned h, + const LodePNGColorMode *mode_in); /*Settings for the encoder.*/ typedef struct LodePNGEncoderSettings { - LodePNGCompressSettings zlibsettings; /*settings for the zlib encoder, such as window size, ...*/ + LodePNGCompressSettings + zlibsettings; /*settings for the zlib encoder, such as window size, ...*/ - unsigned auto_convert; /*automatically choose output PNG color type. Default: true*/ + unsigned auto_convert; /*automatically choose output PNG color type. Default: + true*/ - /*If true, follows the official PNG heuristic: if the PNG uses a palette or lower than - 8 bit depth, set all filters to zero. Otherwise use the filter_strategy. Note that to - completely follow the official PNG heuristic, filter_palette_zero must be true and - filter_strategy must be LFS_MINSUM*/ + /*If true, follows the official PNG heuristic: if the PNG uses a palette or + lower than 8 bit depth, set all filters to zero. Otherwise use the + filter_strategy. Note that to completely follow the official PNG heuristic, + filter_palette_zero must be true and filter_strategy must be LFS_MINSUM*/ unsigned filter_palette_zero; - /*Which filter strategy to use when not using zeroes due to filter_palette_zero. - Set filter_palette_zero to 0 to ensure always using your chosen strategy. Default: LFS_MINSUM*/ + /*Which filter strategy to use when not using zeroes due to + filter_palette_zero. Set filter_palette_zero to 0 to ensure always using your + chosen strategy. Default: LFS_MINSUM*/ LodePNGFilterStrategy filter_strategy; - /*used if filter_strategy is LFS_PREDEFINED. In that case, this must point to a buffer with - the same length as the amount of scanlines in the image, and each value must <= 5. You - have to cleanup this buffer, LodePNG will never free it. Don't forget that filter_palette_zero - must be set to 0 to ensure this is also used on palette or low bitdepth images.*/ - const unsigned char* predefined_filters; + /*used if filter_strategy is LFS_PREDEFINED. In that case, this must point to + a buffer with the same length as the amount of scanlines in the image, and + each value must <= 5. You have to cleanup this buffer, LodePNG will never free + it. Don't forget that filter_palette_zero must be set to 0 to ensure this is + also used on palette or low bitdepth images.*/ + const unsigned char *predefined_filters; /*force creating a PLTE chunk if colortype is 2 or 6 (= a suggested palette). If colortype is 3, PLTE is always created. If color type is explicitely set - to a grayscale type (1 or 4), this is not done and is ignored. If enabling this, - a palette must be present in the info_png. - NOTE: enabling this may worsen compression if auto_convert is used to choose - optimal color mode, because it cannot use grayscale color modes in this case*/ + to a grayscale type (1 or 4), this is not done and is ignored. If enabling + this, a palette must be present in the info_png. NOTE: enabling this may + worsen compression if auto_convert is used to choose optimal color mode, + because it cannot use grayscale color modes in this case*/ unsigned force_palette; #ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS /*add LodePNG identifier and version as a text chunk, for debugging*/ unsigned add_id; - /*encode text chunks as zTXt chunks instead of tEXt chunks, and use compression in iTXt chunks*/ + /*encode text chunks as zTXt chunks instead of tEXt chunks, and use + * compression in iTXt chunks*/ unsigned text_compression; #endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ } LodePNGEncoderSettings; -void lodepng_encoder_settings_init(LodePNGEncoderSettings* settings); +void lodepng_encoder_settings_init(LodePNGEncoderSettings *settings); #endif /*LODEPNG_COMPILE_ENCODER*/ - #if defined(LODEPNG_COMPILE_DECODER) || defined(LODEPNG_COMPILE_ENCODER) /*The settings, state and information for extended encoding and decoding.*/ typedef struct LodePNGState { #ifdef LODEPNG_COMPILE_DECODER LodePNGDecoderSettings decoder; /*the decoding settings*/ -#endif /*LODEPNG_COMPILE_DECODER*/ +#endif /*LODEPNG_COMPILE_DECODER*/ #ifdef LODEPNG_COMPILE_ENCODER LodePNGEncoderSettings encoder; /*the encoding settings*/ -#endif /*LODEPNG_COMPILE_ENCODER*/ - LodePNGColorMode info_raw; /*specifies the format in which you would like to get the raw pixel buffer*/ - LodePNGInfo info_png; /*info of the PNG image obtained after decoding*/ +#endif /*LODEPNG_COMPILE_ENCODER*/ + LodePNGColorMode info_raw; /*specifies the format in which you would like to + get the raw pixel buffer*/ + LodePNGInfo info_png; /*info of the PNG image obtained after decoding*/ unsigned error; } LodePNGState; /*init, cleanup and copy functions to use with this struct*/ -void lodepng_state_init(LodePNGState* state); -void lodepng_state_cleanup(LodePNGState* state); -void lodepng_state_copy(LodePNGState* dest, const LodePNGState* source); -#endif /* defined(LODEPNG_COMPILE_DECODER) || defined(LODEPNG_COMPILE_ENCODER) */ +void lodepng_state_init(LodePNGState *state); +void lodepng_state_cleanup(LodePNGState *state); +void lodepng_state_copy(LodePNGState *dest, const LodePNGState *source); +#endif /* defined(LODEPNG_COMPILE_DECODER) || defined(LODEPNG_COMPILE_ENCODER) \ + */ #ifdef LODEPNG_COMPILE_DECODER /* -Same as lodepng_decode_memory, but uses a LodePNGState to allow custom settings and -getting much more information about the PNG image and color mode. +Same as lodepng_decode_memory, but uses a LodePNGState to allow custom settings +and getting much more information about the PNG image and color mode. */ -unsigned lodepng_decode(unsigned char** out, unsigned* w, unsigned* h, - LodePNGState* state, - const unsigned char* in, size_t insize); +unsigned lodepng_decode(unsigned char **out, unsigned *w, unsigned *h, + LodePNGState *state, const unsigned char *in, + size_t insize); /* Read the PNG header, but not the actual data. This returns only the information that is in the IHDR chunk of the PNG, such as width, height and color type. The information is placed in the info_png field of the LodePNGState. */ -unsigned lodepng_inspect(unsigned* w, unsigned* h, - LodePNGState* state, - const unsigned char* in, size_t insize); +unsigned lodepng_inspect(unsigned *w, unsigned *h, LodePNGState *state, + const unsigned char *in, size_t insize); #endif /*LODEPNG_COMPILE_DECODER*/ /* Reads one metadata chunk (other than IHDR, which is handled by lodepng_inspect) -of the PNG file and outputs what it read in the state. Returns error code on failure. -Use lodepng_inspect first with a new state, then e.g. lodepng_chunk_find_const -to find the desired chunk type, and if non null use lodepng_inspect_chunk (with -chunk_pointer - start_of_file as pos). -Supports most metadata chunks from the PNG standard (gAMA, bKGD, tEXt, ...). -Ignores unsupported, unknown, non-metadata or IHDR chunks (without error). -Requirements: &in[pos] must point to start of a chunk, must use regular -lodepng_inspect first since format of most other chunks depends on IHDR, and if -there is a PLTE chunk, that one must be inspected before tRNS or bKGD. +of the PNG file and outputs what it read in the state. Returns error code on +failure. Use lodepng_inspect first with a new state, then e.g. +lodepng_chunk_find_const to find the desired chunk type, and if non null use +lodepng_inspect_chunk (with chunk_pointer - start_of_file as pos). Supports most +metadata chunks from the PNG standard (gAMA, bKGD, tEXt, ...). Ignores +unsupported, unknown, non-metadata or IHDR chunks (without error). Requirements: +&in[pos] must point to start of a chunk, must use regular lodepng_inspect first +since format of most other chunks depends on IHDR, and if there is a PLTE chunk, +that one must be inspected before tRNS or bKGD. */ -unsigned lodepng_inspect_chunk(LodePNGState* state, size_t pos, - const unsigned char* in, size_t insize); +unsigned lodepng_inspect_chunk(LodePNGState *state, size_t pos, + const unsigned char *in, size_t insize); #ifdef LODEPNG_COMPILE_ENCODER -/*This function allocates the out buffer with standard malloc and stores the size in *outsize.*/ -unsigned lodepng_encode(unsigned char** out, size_t* outsize, - const unsigned char* image, unsigned w, unsigned h, - LodePNGState* state); +/*This function allocates the out buffer with standard malloc and stores the + * size in *outsize.*/ +unsigned lodepng_encode(unsigned char **out, size_t *outsize, + const unsigned char *image, unsigned w, unsigned h, + LodePNGState *state); #endif /*LODEPNG_COMPILE_ENCODER*/ /* @@ -923,13 +1027,13 @@ The chunk pointer always points to the beginning of the chunk itself, that is the first byte of the 4 length bytes. In the PNG file format, chunks have the following format: --4 bytes length: length of the data of the chunk in bytes (chunk itself is 12 bytes longer) --4 bytes chunk type (ASCII a-z,A-Z only, see below) --length bytes of data (may be 0 bytes if length was 0) --4 bytes of CRC, computed on chunk name + data +-4 bytes length: length of the data of the chunk in bytes (chunk itself is 12 +bytes longer) -4 bytes chunk type (ASCII a-z,A-Z only, see below) -length bytes +of data (may be 0 bytes if length was 0) -4 bytes of CRC, computed on chunk name ++ data -The first chunk starts at the 8th byte of the PNG file, the entire rest of the file -exists out of concatenated chunks with the above format. +The first chunk starts at the 8th byte of the PNG file, the entire rest of the +file exists out of concatenated chunks with the above format. PNG standard chunk ASCII naming conventions: -First byte: uppercase = critical, lowercase = ancillary @@ -943,73 +1047,83 @@ Gets the length of the data of the chunk. Total chunk length has 12 bytes more. There must be at least 4 bytes to read from. If the result value is too large, it may be corrupt data. */ -unsigned lodepng_chunk_length(const unsigned char* chunk); +unsigned lodepng_chunk_length(const unsigned char *chunk); /*puts the 4-byte type in null terminated string*/ -void lodepng_chunk_type(char type[5], const unsigned char* chunk); +void lodepng_chunk_type(char type[5], const unsigned char *chunk); /*check if the type is the given type*/ -unsigned char lodepng_chunk_type_equals(const unsigned char* chunk, const char* type); +unsigned char lodepng_chunk_type_equals(const unsigned char *chunk, + const char *type); -/*0: it's one of the critical chunk types, 1: it's an ancillary chunk (see PNG standard)*/ -unsigned char lodepng_chunk_ancillary(const unsigned char* chunk); +/*0: it's one of the critical chunk types, 1: it's an ancillary chunk (see PNG + * standard)*/ +unsigned char lodepng_chunk_ancillary(const unsigned char *chunk); /*0: public, 1: private (see PNG standard)*/ -unsigned char lodepng_chunk_private(const unsigned char* chunk); +unsigned char lodepng_chunk_private(const unsigned char *chunk); -/*0: the chunk is unsafe to copy, 1: the chunk is safe to copy (see PNG standard)*/ -unsigned char lodepng_chunk_safetocopy(const unsigned char* chunk); +/*0: the chunk is unsafe to copy, 1: the chunk is safe to copy (see PNG + * standard)*/ +unsigned char lodepng_chunk_safetocopy(const unsigned char *chunk); -/*get pointer to the data of the chunk, where the input points to the header of the chunk*/ -unsigned char* lodepng_chunk_data(unsigned char* chunk); -const unsigned char* lodepng_chunk_data_const(const unsigned char* chunk); +/*get pointer to the data of the chunk, where the input points to the header of + * the chunk*/ +unsigned char *lodepng_chunk_data(unsigned char *chunk); +const unsigned char *lodepng_chunk_data_const(const unsigned char *chunk); /*returns 0 if the crc is correct, 1 if it's incorrect (0 for OK as usual!)*/ -unsigned lodepng_chunk_check_crc(const unsigned char* chunk); +unsigned lodepng_chunk_check_crc(const unsigned char *chunk); -/*generates the correct CRC from the data and puts it in the last 4 bytes of the chunk*/ -void lodepng_chunk_generate_crc(unsigned char* chunk); +/*generates the correct CRC from the data and puts it in the last 4 bytes of the + * chunk*/ +void lodepng_chunk_generate_crc(unsigned char *chunk); /* Iterate to next chunks, allows iterating through all chunks of the PNG file. -Input must be at the beginning of a chunk (result of a previous lodepng_chunk_next call, -or the 8th byte of a PNG file which always has the first chunk), or alternatively may -point to the first byte of the PNG file (which is not a chunk but the magic header, the -function will then skip over it and return the first real chunk). -Will output pointer to the start of the next chunk, or at or beyond end of the file if there -is no more chunk after this or possibly if the chunk is corrupt. -Start this process at the 8th byte of the PNG file. -In a non-corrupt PNG file, the last chunk should have name "IEND". +Input must be at the beginning of a chunk (result of a previous +lodepng_chunk_next call, or the 8th byte of a PNG file which always has the +first chunk), or alternatively may point to the first byte of the PNG file +(which is not a chunk but the magic header, the function will then skip over it +and return the first real chunk). Will output pointer to the start of the next +chunk, or at or beyond end of the file if there is no more chunk after this or +possibly if the chunk is corrupt. Start this process at the 8th byte of the PNG +file. In a non-corrupt PNG file, the last chunk should have name "IEND". */ -unsigned char* lodepng_chunk_next(unsigned char* chunk, unsigned char* end); -const unsigned char* lodepng_chunk_next_const(const unsigned char* chunk, const unsigned char* end); - -/*Finds the first chunk with the given type in the range [chunk, end), or returns NULL if not found.*/ -unsigned char* lodepng_chunk_find(unsigned char* chunk, unsigned char* end, const char type[5]); -const unsigned char* lodepng_chunk_find_const(const unsigned char* chunk, const unsigned char* end, const char type[5]); +unsigned char *lodepng_chunk_next(unsigned char *chunk, unsigned char *end); +const unsigned char *lodepng_chunk_next_const(const unsigned char *chunk, + const unsigned char *end); + +/*Finds the first chunk with the given type in the range [chunk, end), or + * returns NULL if not found.*/ +unsigned char *lodepng_chunk_find(unsigned char *chunk, unsigned char *end, + const char type[5]); +const unsigned char *lodepng_chunk_find_const(const unsigned char *chunk, + const unsigned char *end, + const char type[5]); /* -Appends chunk to the data in out. The given chunk should already have its chunk header. -The out variable and outsize are updated to reflect the new reallocated buffer. -Returns error code (0 if it went ok) +Appends chunk to the data in out. The given chunk should already have its chunk +header. The out variable and outsize are updated to reflect the new reallocated +buffer. Returns error code (0 if it went ok) */ -unsigned lodepng_chunk_append(unsigned char** out, size_t* outsize, const unsigned char* chunk); +unsigned lodepng_chunk_append(unsigned char **out, size_t *outsize, + const unsigned char *chunk); /* -Appends new chunk to out. The chunk to append is given by giving its length, type -and data separately. The type is a 4-letter string. -The out variable and outsize are updated to reflect the new reallocated buffer. -Returne error code (0 if it went ok) +Appends new chunk to out. The chunk to append is given by giving its length, +type and data separately. The type is a 4-letter string. The out variable and +outsize are updated to reflect the new reallocated buffer. Returne error code (0 +if it went ok) */ -unsigned lodepng_chunk_create(unsigned char** out, size_t* outsize, size_t length, - const char* type, const unsigned char* data); - +unsigned lodepng_chunk_create(unsigned char **out, size_t *outsize, + size_t length, const char *type, + const unsigned char *data); /*Calculate CRC32 of buffer*/ -unsigned lodepng_crc32(const unsigned char* buf, size_t len); +unsigned lodepng_crc32(const unsigned char *buf, size_t len); #endif /*LODEPNG_COMPILE_PNG*/ - #ifdef LODEPNG_COMPILE_ZLIB /* This zlib part can be used independently to zlib compress and decompress a @@ -1018,10 +1132,11 @@ part of zlib that is required for PNG, it does not support dictionaries. */ #ifdef LODEPNG_COMPILE_DECODER -/*Inflate a buffer. Inflate is the decompression step of deflate. Out buffer must be freed after use.*/ -unsigned lodepng_inflate(unsigned char** out, size_t* outsize, - const unsigned char* in, size_t insize, - const LodePNGDecompressSettings* settings); +/*Inflate a buffer. Inflate is the decompression step of deflate. Out buffer + * must be freed after use.*/ +unsigned lodepng_inflate(unsigned char **out, size_t *outsize, + const unsigned char *in, size_t insize, + const LodePNGDecompressSettings *settings); /* Decompresses Zlib data. Reallocates the out buffer and appends the data. The @@ -1029,9 +1144,9 @@ data must be according to the zlib specification. Either, *out must be NULL and *outsize must be 0, or, *out must be a valid buffer and *outsize its size in bytes. out must be freed by user after usage. */ -unsigned lodepng_zlib_decompress(unsigned char** out, size_t* outsize, - const unsigned char* in, size_t insize, - const LodePNGDecompressSettings* settings); +unsigned lodepng_zlib_decompress(unsigned char **out, size_t *outsize, + const unsigned char *in, size_t insize, + const LodePNGDecompressSettings *settings); #endif /*LODEPNG_COMPILE_DECODER*/ #ifdef LODEPNG_COMPILE_ENCODER @@ -1042,21 +1157,23 @@ The data is output in the format of the zlib specification. Either, *out must be NULL and *outsize must be 0, or, *out must be a valid buffer and *outsize its size in bytes. out must be freed by user after usage. */ -unsigned lodepng_zlib_compress(unsigned char** out, size_t* outsize, - const unsigned char* in, size_t insize, - const LodePNGCompressSettings* settings); +unsigned lodepng_zlib_compress(unsigned char **out, size_t *outsize, + const unsigned char *in, size_t insize, + const LodePNGCompressSettings *settings); /* Find length-limited Huffman code for given frequencies. This function is in the public interface only for tests, it's used internally by lodepng_deflate. */ -unsigned lodepng_huffman_code_lengths(unsigned* lengths, const unsigned* frequencies, +unsigned lodepng_huffman_code_lengths(unsigned *lengths, + const unsigned *frequencies, size_t numcodes, unsigned maxbitlen); -/*Compress a buffer with deflate. See RFC 1951. Out buffer must be freed after use.*/ -unsigned lodepng_deflate(unsigned char** out, size_t* outsize, - const unsigned char* in, size_t insize, - const LodePNGCompressSettings* settings); +/*Compress a buffer with deflate. See RFC 1951. Out buffer must be freed after + * use.*/ +unsigned lodepng_deflate(unsigned char **out, size_t *outsize, + const unsigned char *in, size_t insize, + const LodePNGCompressSettings *settings); #endif /*LODEPNG_COMPILE_ENCODER*/ #endif /*LODEPNG_COMPILE_ZLIB*/ @@ -1073,7 +1190,8 @@ return value: error code (0 means ok) NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and decode in-memory. */ -unsigned lodepng_load_file(unsigned char** out, size_t* outsize, const char* filename); +unsigned lodepng_load_file(unsigned char **out, size_t *outsize, + const char *filename); /* Save a file from buffer to disk. Warning, if it exists, this function overwrites @@ -1086,39 +1204,40 @@ return value: error code (0 means ok) NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and encode in-memory */ -unsigned lodepng_save_file(const unsigned char* buffer, size_t buffersize, const char* filename); +unsigned lodepng_save_file(const unsigned char *buffer, size_t buffersize, + const char *filename); #endif /*LODEPNG_COMPILE_DISK*/ #ifdef LODEPNG_COMPILE_CPP -/* The LodePNG C++ wrapper uses std::vectors instead of manually allocated memory buffers. */ +/* The LodePNG C++ wrapper uses std::vectors instead of manually allocated + * memory buffers. */ namespace lodepng { #ifdef LODEPNG_COMPILE_PNG class State : public LodePNGState { - public: - State(); - State(const State& other); - ~State(); - State& operator=(const State& other); +public: + State(); + State(const State &other); + ~State(); + State &operator=(const State &other); }; #ifdef LODEPNG_COMPILE_DECODER -/* Same as other lodepng::decode, but using a State for more settings and information. */ -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - State& state, - const unsigned char* in, size_t insize); -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - State& state, - const std::vector& in); +/* Same as other lodepng::decode, but using a State for more settings and + * information. */ +unsigned decode(std::vector &out, unsigned &w, unsigned &h, + State &state, const unsigned char *in, size_t insize); +unsigned decode(std::vector &out, unsigned &w, unsigned &h, + State &state, const std::vector &in); #endif /*LODEPNG_COMPILE_DECODER*/ #ifdef LODEPNG_COMPILE_ENCODER -/* Same as other lodepng::encode, but using a State for more settings and information. */ -unsigned encode(std::vector& out, - const unsigned char* in, unsigned w, unsigned h, - State& state); -unsigned encode(std::vector& out, - const std::vector& in, unsigned w, unsigned h, - State& state); +/* Same as other lodepng::encode, but using a State for more settings and + * information. */ +unsigned encode(std::vector &out, const unsigned char *in, + unsigned w, unsigned h, State &state); +unsigned encode(std::vector &out, + const std::vector &in, unsigned w, unsigned h, + State &state); #endif /*LODEPNG_COMPILE_ENCODER*/ #ifdef LODEPNG_COMPILE_DISK @@ -1129,38 +1248,48 @@ return value: error code (0 means ok) NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and decode in-memory */ -unsigned load_file(std::vector& buffer, const std::string& filename); +unsigned load_file(std::vector &buffer, + const std::string &filename); /* -Save the binary data in an std::vector to a file on disk. The file is overwritten -without warning. +Save the binary data in an std::vector to a file on disk. The file is +overwritten without warning. NOTE: Wide-character filenames are not supported, you can use an external method to handle such files and encode in-memory */ -unsigned save_file(const std::vector& buffer, const std::string& filename); +unsigned save_file(const std::vector &buffer, + const std::string &filename); #endif /* LODEPNG_COMPILE_DISK */ #endif /* LODEPNG_COMPILE_PNG */ #ifdef LODEPNG_COMPILE_ZLIB #ifdef LODEPNG_COMPILE_DECODER /* Zlib-decompress an unsigned char buffer */ -unsigned decompress(std::vector& out, const unsigned char* in, size_t insize, - const LodePNGDecompressSettings& settings = lodepng_default_decompress_settings); +unsigned decompress(std::vector &out, const unsigned char *in, + size_t insize, + const LodePNGDecompressSettings &settings = + lodepng_default_decompress_settings); /* Zlib-decompress an std::vector */ -unsigned decompress(std::vector& out, const std::vector& in, - const LodePNGDecompressSettings& settings = lodepng_default_decompress_settings); +unsigned decompress(std::vector &out, + const std::vector &in, + const LodePNGDecompressSettings &settings = + lodepng_default_decompress_settings); #endif /* LODEPNG_COMPILE_DECODER */ #ifdef LODEPNG_COMPILE_ENCODER /* Zlib-compress an unsigned char buffer */ -unsigned compress(std::vector& out, const unsigned char* in, size_t insize, - const LodePNGCompressSettings& settings = lodepng_default_compress_settings); +unsigned compress(std::vector &out, const unsigned char *in, + size_t insize, + const LodePNGCompressSettings &settings = + lodepng_default_compress_settings); /* Zlib-compress an std::vector */ -unsigned compress(std::vector& out, const std::vector& in, - const LodePNGCompressSettings& settings = lodepng_default_compress_settings); +unsigned compress(std::vector &out, + const std::vector &in, + const LodePNGCompressSettings &settings = + lodepng_default_compress_settings); #endif /* LODEPNG_COMPILE_ENCODER */ #endif /* LODEPNG_COMPILE_ZLIB */ } /* namespace lodepng */ @@ -1168,24 +1297,26 @@ unsigned compress(std::vector& out, const std::vector (2^31)-1 -[ ] partial decoding (stream processing) -[X] let the "isFullyOpaque" function check color keys and transparent palettes too -[X] better name for the variables "codes", "codesD", "codelengthcodes", "clcl" and "lldl" -[ ] allow treating some errors like warnings, when image is recoverable (e.g. 69, 57, 58) -[ ] make warnings like: oob palette, checksum fail, data after iend, wrong/unknown crit chunk, no null terminator in text, ... -[ ] error messages with line numbers (and version) +[.] test if there are no memory leaks or security exploits - done a lot but +needs to be checked often +[.] check compatibility with various compilers - done but needs to be redone +for every newer version [X] converting color to 16-bit per channel types [X] +support color profile chunk types (but never let them touch RGB values by +default) [ ] support all public PNG chunk types (almost done except sPLT and +hIST) [ ] make sure encoder generates no chunks with size > (2^31)-1 [ ] partial +decoding (stream processing) [X] let the "isFullyOpaque" function check color +keys and transparent palettes too [X] better name for the variables "codes", +"codesD", "codelengthcodes", "clcl" and "lldl" [ ] allow treating some errors +like warnings, when image is recoverable (e.g. 69, 57, 58) [ ] make warnings +like: oob palette, checksum fail, data after iend, wrong/unknown crit chunk, no +null terminator in text, ... [ ] error messages with line numbers (and version) [ ] errors in state instead of as return code? [ ] new errors/warnings like suspiciously big decompressed ztxt or iccp chunk -[ ] let the C++ wrapper catch exceptions coming from the standard library and return LodePNG error codes -[ ] allow user to provide custom color conversion functions, e.g. for premultiplied alpha, padding bits or not, ... -[ ] allow user to give data (void*) to custom allocator -[X] provide alternatives for C library functions not present on some platforms (memcpy, ...) +[ ] let the C++ wrapper catch exceptions coming from the standard library and +return LodePNG error codes [ ] allow user to provide custom color conversion +functions, e.g. for premultiplied alpha, padding bits or not, ... [ ] allow user +to give data (void*) to custom allocator [X] provide alternatives for C library +functions not present on some platforms (memcpy, ...) */ #endif /*LODEPNG_H inclusion guard*/ @@ -1246,7 +1377,8 @@ extra functionality. LodePNG exists out of two files: -lodepng.h: the header file for both C and C++ --lodepng.c(pp): give it the name lodepng.c or lodepng.cpp (or .cc) depending on your usage +-lodepng.c(pp): give it the name lodepng.c or lodepng.cpp (or .cc) depending on +your usage If you want to start using LodePNG right away without reading this doc, get the examples from the LodePNG website to see how to use it in code, or check the @@ -1271,18 +1403,23 @@ begin), life-critical systems, ... The following features are supported by the decoder: -*) decoding of PNGs with any color type, bit depth and interlace mode, to a 24- or 32-bit color raw image, - or the same color type as the PNG -*) encoding of PNGs, from any raw image to 24- or 32-bit color, or the same color type as the raw image +*) decoding of PNGs with any color type, bit depth and interlace mode, to a 24- +or 32-bit color raw image, or the same color type as the PNG +*) encoding of PNGs, from any raw image to 24- or 32-bit color, or the same +color type as the raw image *) Adam7 interlace and deinterlace for any color type -*) loading the image from harddisk or decoding it from a buffer from other sources than harddisk -*) support for alpha channels, including RGBA color model, translucent palettes and color keying +*) loading the image from harddisk or decoding it from a buffer from other +sources than harddisk +*) support for alpha channels, including RGBA color model, translucent palettes +and color keying *) zlib decompression (inflate) *) zlib compression (deflate) *) CRC32 and ADLER32 checksums -*) colorimetric color profile conversions: currently experimentally available in lodepng_util.cpp only, - plus alternatively ability to pass on chroma/gamma/ICC profile information to other color management system. -*) handling of unknown chunks, allowing making a PNG editor that stores custom and unknown chunks. +*) colorimetric color profile conversions: currently experimentally available in +lodepng_util.cpp only, plus alternatively ability to pass on chroma/gamma/ICC +profile information to other color management system. +*) handling of unknown chunks, allowing making a PNG editor that stores custom +and unknown chunks. *) the following chunks are supported by both encoder and decoder: IHDR: header information PLTE: color palette @@ -1307,8 +1444,10 @@ The following features are supported by the decoder: The following features are not (yet) supported: *) some features needed to make a conformant PNG-Editor might be still missing. -*) partial loading/stream processing. All data must be available and is processed in one call. -*) The hIST and sPLT public chunks are not (yet) supported but treated as unknown chunks +*) partial loading/stream processing. All data must be available and is +processed in one call. +*) The hIST and sPLT public chunks are not (yet) supported but treated as +unknown chunks 2. C and C++ version @@ -1342,10 +1481,11 @@ When using LodePNG, care has to be taken with the C version of LodePNG, as well as the C-style structs when working with C++. The following conventions are used for all C-style structs: --if a struct has a corresponding init function, always call the init function when making a new one --if a struct has a corresponding cleanup function, call it before the struct disappears to avoid memory leaks --if a struct has a corresponding copy function, use the copy function instead of "=". - The destination must also be inited already. +-if a struct has a corresponding init function, always call the init function +when making a new one -if a struct has a corresponding cleanup function, call it +before the struct disappears to avoid memory leaks -if a struct has a +corresponding copy function, use the copy function instead of "=". The +destination must also be inited already. 4. Decoding @@ -1361,16 +1501,19 @@ various lodepng::decode functions, and lodepng::State can be used for advanced features. When using the LodePNGState, it uses the following fields for decoding: -*) LodePNGInfo info_png: it stores extra information about the PNG (the input) in here -*) LodePNGColorMode info_raw: here you can say what color mode of the raw image (the output) you want to get -*) LodePNGDecoderSettings decoder: you can specify a few extra settings for the decoder to use +*) LodePNGInfo info_png: it stores extra information about the PNG (the input) +in here +*) LodePNGColorMode info_raw: here you can say what color mode of the raw image +(the output) you want to get +*) LodePNGDecoderSettings decoder: you can specify a few extra settings for the +decoder to use LodePNGInfo info_png -------------------- -After decoding, this contains extra information of the PNG image, except the actual -pixels, width and height because these are already gotten directly from the decoder -functions. +After decoding, this contains extra information of the PNG image, except the +actual pixels, width and height because these are already gotten directly from +the decoder functions. It contains for example the original color type of the PNG image, text comments, suggested background color, etc... More details about the LodePNGInfo struct are @@ -1416,9 +1559,12 @@ since the encoder input is trusted, the decoder input (a PNG image that could be forged by anyone) is not trusted. When using the LodePNGState, it uses the following fields for encoding: -*) LodePNGInfo info_png: here you specify how you want the PNG (the output) to be. -*) LodePNGColorMode info_raw: here you say what color type of the raw image (the input) has -*) LodePNGEncoderSettings encoder: you can specify a few settings for the encoder to use +*) LodePNGInfo info_png: here you specify how you want the PNG (the output) to +be. +*) LodePNGColorMode info_raw: here you say what color type of the raw image (the +input) has +*) LodePNGEncoderSettings encoder: you can specify a few settings for the +encoder to use LodePNGInfo info_png -------------------- @@ -1466,10 +1612,11 @@ can encode the colors of all pixels without information loss. chunk if force_palette is true. This can used as suggested palette to convert to by viewers that don't support more than 256 colors (if those still exist) *) add_id: add text chunk "Encoder: LodePNG " to the image. -*) text_compression: default 1. If 1, it'll store texts as zTXt instead of tEXt chunks. - zTXt chunks use zlib compression on the text. This gives a smaller result on - large texts but a larger result on small texts (such as a single program name). - It's all tEXt or all zTXt though, there's no separate setting per text yet. +*) text_compression: default 1. If 1, it'll store texts as zTXt instead of tEXt +chunks. zTXt chunks use zlib compression on the text. This gives a smaller +result on large texts but a larger result on small texts (such as a single +program name). It's all tEXt or all zTXt though, there's no separate setting per +text yet. 6. color conversions @@ -1548,19 +1695,19 @@ sometimes result in an error. To avoid some confusion: -the decoder converts from PNG to raw image -the encoder converts from raw image to PNG --the colortype and bitdepth in LodePNGColorMode info_raw, are those of the raw image --the colortype and bitdepth in the color field of LodePNGInfo info_png, are those of the PNG --when encoding, the color type in LodePNGInfo is ignored if auto_convert - is enabled, it is automatically generated instead --when decoding, the color type in LodePNGInfo is set by the decoder to that of the original - PNG image, but it can be ignored since the raw image has the color type you requested instead --if the color type of the LodePNGColorMode and PNG image aren't the same, a conversion - between the color types is done if the color types are supported. If it is not - supported, an error is returned. If the types are the same, no conversion is done. --even though some conversions aren't supported, LodePNG supports loading PNGs from any - colortype and saving PNGs to any colortype, sometimes it just requires preparing - the raw image correctly before encoding. --both encoder and decoder use the same color converter. +-the colortype and bitdepth in LodePNGColorMode info_raw, are those of the raw +image -the colortype and bitdepth in the color field of LodePNGInfo info_png, +are those of the PNG -when encoding, the color type in LodePNGInfo is ignored if +auto_convert is enabled, it is automatically generated instead -when decoding, +the color type in LodePNGInfo is set by the decoder to that of the original PNG +image, but it can be ignored since the raw image has the color type you +requested instead -if the color type of the LodePNGColorMode and PNG image +aren't the same, a conversion between the color types is done if the color types +are supported. If it is not supported, an error is returned. If the types are +the same, no conversion is done. -even though some conversions aren't supported, +LodePNG supports loading PNGs from any colortype and saving PNGs to any +colortype, sometimes it just requires preparing the raw image correctly before +encoding. -both encoder and decoder use the same color converter. The function lodepng_convert does the color conversion. It is available in the interface but normally isn't needed since the encoder and decoder already call @@ -1572,8 +1719,8 @@ the result will look ugly because only the red channel is taken (it assumes all three channels are the same in this case so ignores green and blue). The reason no error is given is to allow converting from three-channel grayscale images to one-channel even if there are numerical imprecisions. --anything to palette when the palette does not have an exact match for a from-color -in it: in this case an error is thrown +-anything to palette when the palette does not have an exact match for a +from-color in it: in this case an error is thrown Supported color conversions: -anything to 8-bit RGB, 8-bit RGBA, 16-bit RGB, 16-bit RGBA @@ -1593,13 +1740,14 @@ info_raw are then ignored. 6.3. padding bits ----------------- -In the PNG file format, if a less than 8-bit per pixel color type is used and the scanlines -have a bit amount that isn't a multiple of 8, then padding bits are used so that each -scanline starts at a fresh byte. But that is NOT true for the LodePNG raw input and output. -The raw input image you give to the encoder, and the raw output image you get from the decoder -will NOT have these padding bits, e.g. in the case of a 1-bit image with a width -of 7 pixels, the first pixel of the second scanline will the 8th bit of the first byte, -not the first bit of a new byte. +In the PNG file format, if a less than 8-bit per pixel color type is used and +the scanlines have a bit amount that isn't a multiple of 8, then padding bits +are used so that each scanline starts at a fresh byte. But that is NOT true for +the LodePNG raw input and output. The raw input image you give to the encoder, +and the raw output image you get from the decoder will NOT have these padding +bits, e.g. in the case of a 1-bit image with a width of 7 pixels, the first +pixel of the second scanline will the 8th bit of the first byte, not the first +bit of a new byte. 6.4. A note about 16-bits per channel and endianness ---------------------------------------------------- @@ -1673,7 +1821,8 @@ unsigned lodepng_chunk_length(const unsigned char* chunk): Get the length of the chunk's data. The total chunk length is this length + 12. void lodepng_chunk_type(char type[5], const unsigned char* chunk): -unsigned char lodepng_chunk_type_equals(const unsigned char* chunk, const char* type): +unsigned char lodepng_chunk_type_equals(const unsigned char* chunk, const char* +type): Get the type of the chunk or compare if it's a certain type @@ -1681,11 +1830,12 @@ unsigned char lodepng_chunk_critical(const unsigned char* chunk): unsigned char lodepng_chunk_private(const unsigned char* chunk): unsigned char lodepng_chunk_safetocopy(const unsigned char* chunk): -Check if the chunk is critical in the PNG standard (only IHDR, PLTE, IDAT and IEND are). -Check if the chunk is private (public chunks are part of the standard, private ones not). -Check if the chunk is safe to copy. If it's not, then, when modifying data in a critical -chunk, unsafe to copy chunks of the old image may NOT be saved in the new one if your -program doesn't handle that type of unknown chunk. +Check if the chunk is critical in the PNG standard (only IHDR, PLTE, IDAT and +IEND are). Check if the chunk is private (public chunks are part of the +standard, private ones not). Check if the chunk is safe to copy. If it's not, +then, when modifying data in a critical chunk, unsafe to copy chunks of the old +image may NOT be saved in the new one if your program doesn't handle that type +of unknown chunk. unsigned char* lodepng_chunk_data(unsigned char* chunk): const unsigned char* lodepng_chunk_data_const(const unsigned char* chunk): @@ -1700,18 +1850,19 @@ Check if the crc is correct or generate a correct one. unsigned char* lodepng_chunk_next(unsigned char* chunk): const unsigned char* lodepng_chunk_next_const(const unsigned char* chunk): -Iterate to the next chunk. This works if you have a buffer with consecutive chunks. Note that these -functions do no boundary checking of the allocated data whatsoever, so make sure there is enough -data available in the buffer to be able to go to the next chunk. +Iterate to the next chunk. This works if you have a buffer with consecutive +chunks. Note that these functions do no boundary checking of the allocated data +whatsoever, so make sure there is enough data available in the buffer to be able +to go to the next chunk. -unsigned lodepng_chunk_append(unsigned char** out, size_t* outsize, const unsigned char* chunk): -unsigned lodepng_chunk_create(unsigned char** out, size_t* outsize, unsigned length, - const char* type, const unsigned char* data): +unsigned lodepng_chunk_append(unsigned char** out, size_t* outsize, const +unsigned char* chunk): unsigned lodepng_chunk_create(unsigned char** out, +size_t* outsize, unsigned length, const char* type, const unsigned char* data): -These functions are used to create new chunks that are appended to the data in *out that has -length *outsize. The append function appends an existing chunk to the new data. The create -function creates a new chunk with the given parameters and appends it. Type is the 4-letter -name of the chunk. +These functions are used to create new chunks that are appended to the data in +*out that has length *outsize. The append function appends an existing chunk to +the new data. The create function creates a new chunk with the given parameters +and appends it. Type is the 4-letter name of the chunk. 8.2. chunks in info_png ----------------------- @@ -1830,9 +1981,11 @@ int main(int argc, char *argv[]) { unsigned error = lodepng::decode(image, width, height, filename); //if there's an error, display it - if(error) std::cout << "decoder error " << error << ": " << lodepng_error_text(error) << std::endl; + if(error) std::cout << "decoder error " << error << ": " << +lodepng_error_text(error) << std::endl; - //the pixels are now in the vector "image", 4 bytes per pixel, ordered RGBARGBA..., use it as texture, draw it, ... + //the pixels are now in the vector "image", 4 bytes per pixel, ordered +RGBARGBA..., use it as texture, draw it, ... } 10.2. decoder C example @@ -1867,10 +2020,10 @@ state.decoder.zlibsettings.ignore_adler32: ignore ADLER32 checksums state.decoder.zlibsettings.custom_...: use custom inflate function state.decoder.ignore_crc: ignore CRC checksums state.decoder.ignore_critical: ignore unknown critical chunks -state.decoder.ignore_end: ignore missing IEND chunk. May fail if this corruption causes other errors -state.decoder.color_convert: convert internal PNG color to chosen one -state.decoder.read_text_chunks: whether to read in text metadata chunks -state.decoder.remember_unknown_chunks: whether to read in unknown chunks +state.decoder.ignore_end: ignore missing IEND chunk. May fail if this corruption +causes other errors state.decoder.color_convert: convert internal PNG color to +chosen one state.decoder.read_text_chunks: whether to read in text metadata +chunks state.decoder.remember_unknown_chunks: whether to read in unknown chunks state.info_raw.colortype: desired color type for decoded image state.info_raw.bitdepth: desired bit depth for decoded image state.info_raw....: more color settings, see struct LodePNGColorMode @@ -2042,18 +2195,20 @@ Not all changes are listed here, the commit history in github lists more: *) 29 dec 2006: Added support for encoding images without alpha channel, and cleaned out code as well as making certain parts faster. *) 28 dec 2006: Added "Settings" to the encoder. -*) 26 dec 2006: The encoder now does LZ77 encoding and produces much smaller files now. - Removed some code duplication in the decoder. Fixed little bug in an example. -*) 09 dec 2006: (!) Placed output parameters of public functions as first parameter. - Fixed a bug of the decoder with 16-bit per color. +*) 26 dec 2006: The encoder now does LZ77 encoding and produces much smaller +files now. Removed some code duplication in the decoder. Fixed little bug in an +example. +*) 09 dec 2006: (!) Placed output parameters of public functions as first +parameter. Fixed a bug of the decoder with 16-bit per color. *) 15 okt 2006: Changed documentation structure *) 09 okt 2006: Encoder class added. It encodes a valid PNG image from the given image buffer, however for now it's not compressed. *) 08 sep 2006: (!) Changed to interface with a Decoder class -*) 30 jul 2006: (!) LodePNG_InfoPng , width and height are now retrieved in different - way. Renamed decodePNG to decodePNGGeneric. +*) 30 jul 2006: (!) LodePNG_InfoPng , width and height are now retrieved in +different way. Renamed decodePNG to decodePNGGeneric. *) 29 jul 2006: (!) Changed the interface: image info is now returned as a - struct of type LodePNG::LodePNG_Info, instead of a vector, which was a bit clumsy. + struct of type LodePNG::LodePNG_Info, instead of a vector, which was a bit +clumsy. *) 28 jul 2006: Cleaned the code and added new error checks. Corrected terminology "deflate" into "inflate". *) 23 jun 2006: Added SDL example in the documentation in the header, this diff --git a/simulator/mujoco/draco/simulate/platform_ui_adapter.h b/simulator/mujoco/draco/simulate/platform_ui_adapter.h index 64b0211d..b8e6bd4e 100644 --- a/simulator/mujoco/draco/simulate/platform_ui_adapter.h +++ b/simulator/mujoco/draco/simulate/platform_ui_adapter.h @@ -21,25 +21,25 @@ namespace mujoco { class PlatformUIAdapter { - public: +public: virtual ~PlatformUIAdapter() = default; - inline mjuiState& state() { return state_; } - inline const mjuiState& state() const { return state_; } + inline mjuiState &state() { return state_; } + inline const mjuiState &state() const { return state_; } - inline mjrContext& mjr_context() { return con_; } - inline const mjrContext& mjr_context() const { return con_; } + inline mjrContext &mjr_context() { return con_; } + inline const mjrContext &mjr_context() const { return con_; } - inline void SetEventCallback(void (*event_callback)(mjuiState*)) { + inline void SetEventCallback(void (*event_callback)(mjuiState *)) { event_callback_ = event_callback; } - inline void SetLayoutCallback(void (*layout_callback)(mjuiState*)) { + inline void SetLayoutCallback(void (*layout_callback)(mjuiState *)) { layout_callback_ = layout_callback; } // Optionally overridable function to (re)create an mjrContext for an mjModel - virtual bool RefreshMjrContext(const mjModel* m, int fontscale); + virtual bool RefreshMjrContext(const mjModel *m, int fontscale); virtual bool EnsureContextSize(); @@ -50,9 +50,9 @@ class PlatformUIAdapter { virtual std::pair GetWindowSize() const = 0; virtual bool IsGPUAccelerated() const = 0; virtual void PollEvents() = 0; - virtual void SetClipboardString(const char* text) = 0; + virtual void SetClipboardString(const char *text) = 0; virtual void SetVSync(bool enabled) = 0; - virtual void SetWindowTitle(const char* title) = 0; + virtual void SetWindowTitle(const char *title) = 0; virtual bool ShouldCloseWindow() const = 0; virtual void SwapBuffers() = 0; virtual void ToggleFullscreen() = 0; @@ -71,12 +71,12 @@ class PlatformUIAdapter { virtual int TranslateKeyCode(int key) const = 0; virtual mjtButton TranslateMouseButton(int button) const = 0; - protected: +protected: PlatformUIAdapter(); void FreeMjrContext(); // Event handlers - void OnFilesDrop(int count, const char** paths); + void OnFilesDrop(int count, const char **paths); virtual void OnKey(int key, int scancode, int act); void OnMouseButton(int button, int act); void OnMouseMove(double x, double y); @@ -86,16 +86,16 @@ class PlatformUIAdapter { mjuiState state_; int last_key_; - void (*event_callback_)(mjuiState*); - void (*layout_callback_)(mjuiState*); + void (*event_callback_)(mjuiState *); + void (*layout_callback_)(mjuiState *); mjrContext con_; - const mjModel* last_model_ = nullptr; + const mjModel *last_model_ = nullptr; int last_fontscale_ = -1; - private: +private: void UpdateMjuiState(); }; -} // namespace mujoco +} // namespace mujoco -#endif // MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_ +#endif // MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_ diff --git a/simulator/pybullet/draco_main.py b/simulator/pybullet/draco_main.py index 0bc3617c..65062af1 100644 --- a/simulator/pybullet/draco_main.py +++ b/simulator/pybullet/draco_main.py @@ -1,5 +1,4 @@ import pybullet as pb -import time import os import sys import numpy as np @@ -45,8 +44,7 @@ def get_sensor_data_from_pybullet(robot): # follow pinocchio robotsystem urdf reading convention joint_pos, joint_vel = np.zeros(27), np.zeros(27) - imu_frame_quat = np.array( - pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[1]) + imu_frame_quat = np.array(pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[1]) # LF joint_pos[0] = pb.getJointState(robot, DracoJointIdx.l_hip_ie)[0] joint_pos[1] = pb.getJointState(robot, DracoJointIdx.l_hip_aa)[0] @@ -80,11 +78,11 @@ def get_sensor_data_from_pybullet(robot): joint_pos[25] = pb.getJointState(robot, DracoJointIdx.r_wrist_ps)[0] joint_pos[26] = pb.getJointState(robot, DracoJointIdx.r_wrist_pitch)[0] - imu_ang_vel = np.array( - pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[7]) + imu_ang_vel = np.array(pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[7]) - imu_dvel = pybullet_util.simulate_dVel_data(robot, link_id_dict, - previous_torso_velocity) + imu_dvel = pybullet_util.simulate_dVel_data( + robot, link_id_dict, previous_torso_velocity + ) # LF joint_vel[0] = pb.getJointState(robot, DracoJointIdx.l_hip_ie)[1] @@ -121,23 +119,27 @@ def get_sensor_data_from_pybullet(robot): # normal force measured on each foot _l_normal_force = 0 - contacts = pb.getContactPoints(bodyA=robot, - linkIndexA=DracoLinkIdx.l_ankle_ie_link) + contacts = pb.getContactPoints(bodyA=robot, linkIndexA=DracoLinkIdx.l_ankle_ie_link) for contact in contacts: # add z-component on all points of contact _l_normal_force += contact[9] _r_normal_force = 0 - contacts = pb.getContactPoints(bodyA=robot, - linkIndexA=DracoLinkIdx.r_ankle_ie_link) + contacts = pb.getContactPoints(bodyA=robot, linkIndexA=DracoLinkIdx.r_ankle_ie_link) for contact in contacts: # add z-component on all points of contact _r_normal_force += contact[9] - b_lf_contact = (True if pb.getLinkState(robot, DracoLinkIdx.l_foot_contact, - 1, 1)[0][2] <= 0.05 else False) - b_rf_contact = (True if pb.getLinkState(robot, DracoLinkIdx.r_foot_contact, - 1, 1)[0][2] <= 0.05 else False) + b_lf_contact = ( + True + if pb.getLinkState(robot, DracoLinkIdx.l_foot_contact, 1, 1)[0][2] <= 0.05 + else False + ) + b_rf_contact = ( + True + if pb.getLinkState(robot, DracoLinkIdx.r_foot_contact, 1, 1)[0][2] <= 0.05 + else False + ) return ( imu_frame_quat, imu_ang_vel, @@ -156,116 +158,91 @@ def apply_control_input_to_pybullet(robot, command): mode = pb.TORQUE_CONTROL # LF - pb.setJointMotorControl2(robot, - DracoJointIdx.l_hip_ie, - controlMode=mode, - force=command[0]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_hip_aa, - controlMode=mode, - force=command[1]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_hip_fe, - controlMode=mode, - force=command[2]) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_hip_ie, controlMode=mode, force=command[0] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_hip_aa, controlMode=mode, force=command[1] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_hip_fe, controlMode=mode, force=command[2] + ) # pb.setJointMotorControl2(robot, DracoJointIdx.l_knee_fe_jp, controlMode=mode, force=command[3]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_knee_fe_jd, - controlMode=mode, - force=command[4]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_ankle_fe, - controlMode=mode, - force=command[5]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_ankle_ie, - controlMode=mode, - force=command[6]) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_knee_fe_jd, controlMode=mode, force=command[4] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_ankle_fe, controlMode=mode, force=command[5] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_ankle_ie, controlMode=mode, force=command[6] + ) # LH - pb.setJointMotorControl2(robot, - DracoJointIdx.l_shoulder_fe, - controlMode=mode, - force=command[7]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_shoulder_aa, - controlMode=mode, - force=command[8]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_shoulder_ie, - controlMode=mode, - force=command[9]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_elbow_fe, - controlMode=mode, - force=command[10]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_wrist_ps, - controlMode=mode, - force=command[11]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_wrist_pitch, - controlMode=mode, - force=command[12]) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_shoulder_fe, controlMode=mode, force=command[7] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_shoulder_aa, controlMode=mode, force=command[8] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_shoulder_ie, controlMode=mode, force=command[9] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_elbow_fe, controlMode=mode, force=command[10] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_wrist_ps, controlMode=mode, force=command[11] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_wrist_pitch, controlMode=mode, force=command[12] + ) # neck - pb.setJointMotorControl2(robot, - DracoJointIdx.neck_pitch, - controlMode=mode, - force=command[13]) + pb.setJointMotorControl2( + robot, DracoJointIdx.neck_pitch, controlMode=mode, force=command[13] + ) # RF - pb.setJointMotorControl2(robot, - DracoJointIdx.r_hip_ie, - controlMode=mode, - force=command[14]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_hip_aa, - controlMode=mode, - force=command[15]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_hip_fe, - controlMode=mode, - force=command[16]) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_hip_ie, controlMode=mode, force=command[14] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_hip_aa, controlMode=mode, force=command[15] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_hip_fe, controlMode=mode, force=command[16] + ) # pb.setJointMotorControl2(robot, DracoJointIdx.r_knee_fe_jd, controlMode=mode, force=command[17]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_knee_fe_jd, - controlMode=mode, - force=command[18]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_ankle_fe, - controlMode=mode, - force=command[19]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_ankle_ie, - controlMode=mode, - force=command[20]) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_knee_fe_jd, controlMode=mode, force=command[18] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_ankle_fe, controlMode=mode, force=command[19] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_ankle_ie, controlMode=mode, force=command[20] + ) # RH - pb.setJointMotorControl2(robot, - DracoJointIdx.r_shoulder_fe, - controlMode=mode, - force=command[21]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_shoulder_aa, - controlMode=mode, - force=command[22]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_shoulder_ie, - controlMode=mode, - force=command[23]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_elbow_fe, - controlMode=mode, - force=command[24]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_wrist_ps, - controlMode=mode, - force=command[25]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_wrist_pitch, - controlMode=mode, - force=command[26]) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_shoulder_fe, controlMode=mode, force=command[21] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_shoulder_aa, controlMode=mode, force=command[22] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_shoulder_ie, controlMode=mode, force=command[23] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_elbow_fe, controlMode=mode, force=command[24] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_wrist_ps, controlMode=mode, force=command[25] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_wrist_pitch, controlMode=mode, force=command[26] + ) def set_init_config_pybullet_robot(robot): @@ -277,23 +254,19 @@ def set_init_config_pybullet_robot(robot): # Lowerbody hip_yaw_angle = 0 - pb.resetJointState(robot, DracoJointIdx.l_hip_aa, - np.radians(hip_yaw_angle), 0.0) + pb.resetJointState(robot, DracoJointIdx.l_hip_aa, np.radians(hip_yaw_angle), 0.0) pb.resetJointState(robot, DracoJointIdx.l_hip_fe, -np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.l_knee_fe_jp, np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.l_knee_fe_jd, np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.l_ankle_fe, -np.pi / 4, 0.0) - pb.resetJointState(robot, DracoJointIdx.l_ankle_ie, - np.radians(-hip_yaw_angle), 0.0) + pb.resetJointState(robot, DracoJointIdx.l_ankle_ie, np.radians(-hip_yaw_angle), 0.0) - pb.resetJointState(robot, DracoJointIdx.r_hip_aa, - np.radians(-hip_yaw_angle), 0.0) + pb.resetJointState(robot, DracoJointIdx.r_hip_aa, np.radians(-hip_yaw_angle), 0.0) pb.resetJointState(robot, DracoJointIdx.r_hip_fe, -np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.r_knee_fe_jp, np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.r_knee_fe_jd, np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.r_ankle_fe, -np.pi / 4, 0.0) - pb.resetJointState(robot, DracoJointIdx.r_ankle_ie, - np.radians(hip_yaw_angle), 0.0) + pb.resetJointState(robot, DracoJointIdx.r_ankle_ie, np.radians(hip_yaw_angle), 0.0) def signal_handler(signal, frame): @@ -303,9 +276,9 @@ def signal_handler(signal, frame): print("========================================================") print('saving list of compuation time in "compuation_time.txt"') print("========================================================") - np.savetxt("computation_time.txt", - np.array([compuation_cal_list]), - delimiter=",") + np.savetxt( + "computation_time.txt", np.array([compuation_cal_list]), delimiter="," + ) if Config.VIDEO_RECORD: print("========================================================") @@ -331,8 +304,9 @@ def signal_handler(signal, frame): cameraTargetPosition=[0, 0, 0.5], ) ## sim physics setting - pb.setPhysicsEngineParameter(fixedTimeStep=Config.CONTROLLER_DT, - numSubSteps=Config.N_SUBSTEP) + pb.setPhysicsEngineParameter( + fixedTimeStep=Config.CONTROLLER_DT, numSubSteps=Config.N_SUBSTEP + ) pb.setGravity(0, 0, -9.81) ## robot spawn & initial kinematics and dynamics setting @@ -348,8 +322,7 @@ def signal_handler(signal, frame): useFixedBase=0, ) - ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", - useFixedBase=1) + ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", useFixedBase=1) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) # TODO:modify this function without dictionary container @@ -412,18 +385,17 @@ def signal_handler(signal, frame): active_jointidx_list = [] # for setjointmotorcontrolarray # default robot kinematics information - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(np.array(base_com_quat)) - rot_world_basejoint = util.quat_to_rot( - np.array(Config.INITIAL_BASE_JOINT_QUAT)) + rot_world_basejoint = util.quat_to_rot(np.array(Config.INITIAL_BASE_JOINT_QUAT)) pos_basejoint_to_basecom = np.dot( rot_world_basejoint.transpose(), base_com_pos - np.array(Config.INITIAL_BASE_JOINT_POS), ) - rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - rot_world_basecom) + rot_basejoint_to_basecom = np.dot( + rot_world_basejoint.transpose(), rot_world_basecom + ) # Run Simulation dt = Config.CONTROLLER_DT @@ -453,28 +425,31 @@ def signal_handler(signal, frame): # Moving Camera Setting ############################################################ base_pos, base_ori = pb.getBasePositionAndOrientation(draco_humanoid) - pb.resetDebugVisualizerCamera(cameraDistance=1.5, - cameraYaw=120, - cameraPitch=-30, - cameraTargetPosition=base_pos + - np.array([0.5, 0.3, -base_pos[2] + 1])) + pb.resetDebugVisualizerCamera( + cameraDistance=1.5, + cameraYaw=120, + cameraPitch=-30, + cameraTargetPosition=base_pos + np.array([0.5, 0.3, -base_pos[2] + 1]), + ) ############################################################################### # Debugging Purpose ############################################################################## ##debugging state estimator by calculating groundtruth basejoint states - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(base_com_quat) - rot_world_basejoint = np.dot(rot_world_basecom, - rot_basejoint_to_basecom.transpose()) - base_joint_pos = base_com_pos - np.dot(rot_world_basejoint, - pos_basejoint_to_basecom) + rot_world_basejoint = np.dot( + rot_world_basecom, rot_basejoint_to_basecom.transpose() + ) + base_joint_pos = base_com_pos - np.dot( + rot_world_basejoint, pos_basejoint_to_basecom + ) base_joint_quat = util.rot_to_quat(rot_world_basejoint) base_com_lin_vel, base_com_ang_vel = pb.getBaseVelocity(draco_humanoid) - trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, - pos_basejoint_to_basecom) + trans_joint_com = liegroup.RpToTrans( + rot_basejoint_to_basecom, pos_basejoint_to_basecom + ) adjoint_joint_com = liegroup.Adjoint(trans_joint_com) twist_basecom_in_world = np.zeros(6) twist_basecom_in_world[0:3] = base_com_ang_vel @@ -482,15 +457,16 @@ def signal_handler(signal, frame): augrot_basecom_world = np.zeros((6, 6)) augrot_basecom_world[0:3, 0:3] = rot_world_basecom.transpose() augrot_basecom_world[3:6, 3:6] = rot_world_basecom.transpose() - twist_basecom_in_basecom = np.dot(augrot_basecom_world, - twist_basecom_in_world) - twist_basejoint_in_basejoint = np.dot(adjoint_joint_com, - twist_basecom_in_basecom) + twist_basecom_in_basecom = np.dot(augrot_basecom_world, twist_basecom_in_world) + twist_basejoint_in_basejoint = np.dot( + adjoint_joint_com, twist_basecom_in_basecom + ) augrot_world_basejoint = np.zeros((6, 6)) augrot_world_basejoint[0:3, 0:3] = rot_world_basejoint augrot_world_basejoint[3:6, 3:6] = rot_world_basejoint - twist_basejoint_in_world = np.dot(augrot_world_basejoint, - twist_basejoint_in_basejoint) + twist_basejoint_in_world = np.dot( + augrot_world_basejoint, twist_basejoint_in_basejoint + ) base_joint_ang_vel = twist_basejoint_in_world[0:3] base_joint_lin_vel = twist_basejoint_in_world[3:6] @@ -544,12 +520,13 @@ def signal_handler(signal, frame): l_normal_force = pybullet_util.simulate_contact_sensor(l_normal_force) r_normal_force = pybullet_util.simulate_contact_sensor(r_normal_force) imu_dvel = pybullet_util.add_sensor_noise(imu_dvel, imu_dvel_bias) - imu_ang_vel = pybullet_util.add_sensor_noise(imu_ang_vel, - imu_ang_vel_noise) + imu_ang_vel = pybullet_util.add_sensor_noise(imu_ang_vel, imu_ang_vel_noise) l_normal_force = pybullet_util.add_sensor_noise( - l_normal_force, l_normal_volt_noise) + l_normal_force, l_normal_volt_noise + ) r_normal_force = pybullet_util.add_sensor_noise( - r_normal_force, r_normal_volt_noise) + r_normal_force, r_normal_volt_noise + ) # copy sensor data to rpc sensor data class rpc_draco_sensor_data.imu_frame_quat_ = imu_frame_quat @@ -569,8 +546,7 @@ def signal_handler(signal, frame): if Config.MEASURE_COMPUTATION_TIME: timer.tic() - rpc_draco_interface.GetCommand(rpc_draco_sensor_data, - rpc_draco_command) + rpc_draco_interface.GetCommand(rpc_draco_sensor_data, rpc_draco_command) if Config.MEASURE_COMPUTATION_TIME: comp_time = timer.tocvalue() @@ -587,7 +563,8 @@ def signal_handler(signal, frame): # lfoot_pos = pybullet_util.get_link_iso(draco_humanoid, # save current torso velocity for next iteration previous_torso_velocity = pybullet_util.get_link_vel( - draco_humanoid, link_id_dict["torso_imu"])[3:6] + draco_humanoid, link_id_dict["torso_imu"] + )[3:6] # DracoLinkIdx.l_foot_contact)[0:3, 3] # rfoot_pos = pybullet_util.get_link_iso(draco_humanoid, @@ -606,7 +583,8 @@ def signal_handler(signal, frame): if (Config.VIDEO_RECORD) and (count % Config.RECORD_FREQ == 0): camera_data = pb.getDebugVisualizerCamera() frame = pybullet_util.get_camera_image_from_debug_camera( - camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT) + camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT + ) filename = video_dir + "/step%06d.jpg" % jpg_count cv2.imwrite(filename, frame) jpg_count += 1 diff --git a/simulator/pybullet/draco_main_wbic.py b/simulator/pybullet/draco_main_wbic.py index 59fdd2bf..db467ef4 100644 --- a/simulator/pybullet/draco_main_wbic.py +++ b/simulator/pybullet/draco_main_wbic.py @@ -1,24 +1,20 @@ import pybullet as pb -import time import os cwd = os.getcwd() import sys sys.path.append(cwd) -sys.path.append(cwd + "/build/lib") #include pybind module +sys.path.append(cwd + "/build/lib") # include pybind module -import time import numpy as np -import ipdb from config.draco.sim.pybullet.wbic.pybullet_params import * from util.python_utils import pybullet_util from util.python_utils import util from util.python_utils import liegroup -import copy import signal import shutil @@ -33,487 +29,483 @@ def get_sensor_data_from_pybullet(robot): - - #follow pinocchio robotsystem urdf reading convention + # follow pinocchio robotsystem urdf reading convention joint_pos, joint_vel = np.zeros(27), np.zeros(27) - imu_frame_quat = np.array( - pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[1]) - #LF + imu_frame_quat = np.array(pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[1]) + # LF joint_pos[0] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_ie)[0] joint_pos[1] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_aa)[0] joint_pos[2] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_fe)[0] - joint_pos[3] = pb.getJointState(robot, - PybulletDracoJointIdx.l_knee_fe_jp)[0] - joint_pos[4] = pb.getJointState(robot, - PybulletDracoJointIdx.l_knee_fe_jd)[0] + joint_pos[3] = pb.getJointState(robot, PybulletDracoJointIdx.l_knee_fe_jp)[0] + joint_pos[4] = pb.getJointState(robot, PybulletDracoJointIdx.l_knee_fe_jd)[0] joint_pos[5] = pb.getJointState(robot, PybulletDracoJointIdx.l_ankle_fe)[0] joint_pos[6] = pb.getJointState(robot, PybulletDracoJointIdx.l_ankle_ie)[0] - #LH - joint_pos[7] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_fe)[0] - joint_pos[8] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_aa)[0] - joint_pos[9] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_ie)[0] - joint_pos[10] = pb.getJointState(robot, - PybulletDracoJointIdx.l_elbow_fe)[0] - joint_pos[11] = pb.getJointState(robot, - PybulletDracoJointIdx.l_wrist_ps)[0] - joint_pos[12] = pb.getJointState(robot, - PybulletDracoJointIdx.l_wrist_pitch)[0] - #neck - joint_pos[13] = pb.getJointState(robot, - PybulletDracoJointIdx.neck_pitch)[0] - #RF + # LH + joint_pos[7] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_fe)[0] + joint_pos[8] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_aa)[0] + joint_pos[9] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_ie)[0] + joint_pos[10] = pb.getJointState(robot, PybulletDracoJointIdx.l_elbow_fe)[0] + joint_pos[11] = pb.getJointState(robot, PybulletDracoJointIdx.l_wrist_ps)[0] + joint_pos[12] = pb.getJointState(robot, PybulletDracoJointIdx.l_wrist_pitch)[0] + # neck + joint_pos[13] = pb.getJointState(robot, PybulletDracoJointIdx.neck_pitch)[0] + # RF joint_pos[14] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_ie)[0] joint_pos[15] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_aa)[0] joint_pos[16] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_fe)[0] - joint_pos[17] = pb.getJointState(robot, - PybulletDracoJointIdx.r_knee_fe_jp)[0] - joint_pos[18] = pb.getJointState(robot, - PybulletDracoJointIdx.r_knee_fe_jd)[0] - joint_pos[19] = pb.getJointState(robot, - PybulletDracoJointIdx.r_ankle_fe)[0] - joint_pos[20] = pb.getJointState(robot, - PybulletDracoJointIdx.r_ankle_ie)[0] - #RH - joint_pos[21] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_fe)[0] - joint_pos[22] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_aa)[0] - joint_pos[23] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_ie)[0] - joint_pos[24] = pb.getJointState(robot, - PybulletDracoJointIdx.r_elbow_fe)[0] - joint_pos[25] = pb.getJointState(robot, - PybulletDracoJointIdx.r_wrist_ps)[0] - joint_pos[26] = pb.getJointState(robot, - PybulletDracoJointIdx.r_wrist_pitch)[0] - - imu_ang_vel = np.array( - pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[7]) - - imu_dvel = pybullet_util.simulate_dVel_data(robot, link_id_dict, - previous_torso_velocity) - - #LF + joint_pos[17] = pb.getJointState(robot, PybulletDracoJointIdx.r_knee_fe_jp)[0] + joint_pos[18] = pb.getJointState(robot, PybulletDracoJointIdx.r_knee_fe_jd)[0] + joint_pos[19] = pb.getJointState(robot, PybulletDracoJointIdx.r_ankle_fe)[0] + joint_pos[20] = pb.getJointState(robot, PybulletDracoJointIdx.r_ankle_ie)[0] + # RH + joint_pos[21] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_fe)[0] + joint_pos[22] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_aa)[0] + joint_pos[23] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_ie)[0] + joint_pos[24] = pb.getJointState(robot, PybulletDracoJointIdx.r_elbow_fe)[0] + joint_pos[25] = pb.getJointState(robot, PybulletDracoJointIdx.r_wrist_ps)[0] + joint_pos[26] = pb.getJointState(robot, PybulletDracoJointIdx.r_wrist_pitch)[0] + + imu_ang_vel = np.array(pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[7]) + + imu_dvel = pybullet_util.simulate_dVel_data( + robot, link_id_dict, previous_torso_velocity + ) + + # LF joint_vel[0] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_ie)[1] joint_vel[1] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_aa)[1] joint_vel[2] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_fe)[1] - joint_vel[3] = pb.getJointState(robot, - PybulletDracoJointIdx.l_knee_fe_jp)[1] - joint_vel[4] = pb.getJointState(robot, - PybulletDracoJointIdx.l_knee_fe_jd)[1] + joint_vel[3] = pb.getJointState(robot, PybulletDracoJointIdx.l_knee_fe_jp)[1] + joint_vel[4] = pb.getJointState(robot, PybulletDracoJointIdx.l_knee_fe_jd)[1] joint_vel[5] = pb.getJointState(robot, PybulletDracoJointIdx.l_ankle_fe)[1] joint_vel[6] = pb.getJointState(robot, PybulletDracoJointIdx.l_ankle_ie)[1] - #LH - joint_vel[7] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_fe)[1] - joint_vel[8] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_aa)[1] - joint_vel[9] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_ie)[1] - joint_vel[10] = pb.getJointState(robot, - PybulletDracoJointIdx.l_elbow_fe)[1] - joint_vel[11] = pb.getJointState(robot, - PybulletDracoJointIdx.l_wrist_ps)[1] - joint_vel[12] = pb.getJointState(robot, - PybulletDracoJointIdx.l_wrist_pitch)[1] - #neck - joint_vel[13] = pb.getJointState(robot, - PybulletDracoJointIdx.neck_pitch)[1] - #RF + # LH + joint_vel[7] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_fe)[1] + joint_vel[8] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_aa)[1] + joint_vel[9] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_ie)[1] + joint_vel[10] = pb.getJointState(robot, PybulletDracoJointIdx.l_elbow_fe)[1] + joint_vel[11] = pb.getJointState(robot, PybulletDracoJointIdx.l_wrist_ps)[1] + joint_vel[12] = pb.getJointState(robot, PybulletDracoJointIdx.l_wrist_pitch)[1] + # neck + joint_vel[13] = pb.getJointState(robot, PybulletDracoJointIdx.neck_pitch)[1] + # RF joint_vel[14] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_ie)[1] joint_vel[15] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_aa)[1] joint_vel[16] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_fe)[1] - joint_vel[17] = pb.getJointState(robot, - PybulletDracoJointIdx.r_knee_fe_jp)[1] - joint_vel[18] = pb.getJointState(robot, - PybulletDracoJointIdx.r_knee_fe_jd)[1] - joint_vel[19] = pb.getJointState(robot, - PybulletDracoJointIdx.r_ankle_fe)[1] - joint_vel[20] = pb.getJointState(robot, - PybulletDracoJointIdx.r_ankle_ie)[1] - #RH - joint_vel[21] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_fe)[1] - joint_vel[22] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_aa)[1] - joint_vel[23] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_ie)[1] - joint_vel[24] = pb.getJointState(robot, - PybulletDracoJointIdx.r_elbow_fe)[1] - joint_vel[25] = pb.getJointState(robot, - PybulletDracoJointIdx.r_wrist_ps)[1] - joint_vel[26] = pb.getJointState(robot, - PybulletDracoJointIdx.r_wrist_pitch)[1] - - b_lf_contact = True if pb.getLinkState(robot, DracoLinkIdx.l_foot_contact, - 1, 1)[0][2] <= 0.05 else False - b_rf_contact = True if pb.getLinkState(robot, DracoLinkIdx.r_foot_contact, - 1, 1)[0][2] <= 0.05 else False - return imu_frame_quat, imu_ang_vel, imu_dvel, joint_pos, joint_vel, b_lf_contact, b_rf_contact - - -#TODO:try to modify with setjointmotorcontrol "array" API + joint_vel[17] = pb.getJointState(robot, PybulletDracoJointIdx.r_knee_fe_jp)[1] + joint_vel[18] = pb.getJointState(robot, PybulletDracoJointIdx.r_knee_fe_jd)[1] + joint_vel[19] = pb.getJointState(robot, PybulletDracoJointIdx.r_ankle_fe)[1] + joint_vel[20] = pb.getJointState(robot, PybulletDracoJointIdx.r_ankle_ie)[1] + # RH + joint_vel[21] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_fe)[1] + joint_vel[22] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_aa)[1] + joint_vel[23] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_ie)[1] + joint_vel[24] = pb.getJointState(robot, PybulletDracoJointIdx.r_elbow_fe)[1] + joint_vel[25] = pb.getJointState(robot, PybulletDracoJointIdx.r_wrist_ps)[1] + joint_vel[26] = pb.getJointState(robot, PybulletDracoJointIdx.r_wrist_pitch)[1] + + b_lf_contact = ( + True + if pb.getLinkState(robot, DracoLinkIdx.l_foot_contact, 1, 1)[0][2] <= 0.05 + else False + ) + b_rf_contact = ( + True + if pb.getLinkState(robot, DracoLinkIdx.r_foot_contact, 1, 1)[0][2] <= 0.05 + else False + ) + return ( + imu_frame_quat, + imu_ang_vel, + imu_dvel, + joint_pos, + joint_vel, + b_lf_contact, + b_rf_contact, + ) + + +# TODO:try to modify with setjointmotorcontrol "array" API def apply_torque_control_to_pybullet(robot, command): mode = pb.TORQUE_CONTROL - #LF - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_hip_ie, - controlMode=mode, - force=command[0]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_hip_aa, - controlMode=mode, - force=command[1]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_hip_fe, - controlMode=mode, - force=command[2]) + # LF + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_hip_ie, controlMode=mode, force=command[0] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_hip_aa, controlMode=mode, force=command[1] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_hip_fe, controlMode=mode, force=command[2] + ) # pb.setJointMotorControl2(robot, PybulletDracoJointIdx.l_knee_fe_jp, controlMode=mode, force=command[3]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_knee_fe_jd, - controlMode=mode, - force=command[4]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_ankle_fe, - controlMode=mode, - force=command[5]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_ankle_ie, - controlMode=mode, - force=command[6]) - - #LH - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_shoulder_fe, - controlMode=mode, - force=command[7]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_shoulder_aa, - controlMode=mode, - force=command[8]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_shoulder_ie, - controlMode=mode, - force=command[9]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_elbow_fe, - controlMode=mode, - force=command[10]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_wrist_ps, - controlMode=mode, - force=command[11]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_wrist_pitch, - controlMode=mode, - force=command[12]) - - #neck - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.neck_pitch, - controlMode=mode, - force=command[13]) - - #RF - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_hip_ie, - controlMode=mode, - force=command[14]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_hip_aa, - controlMode=mode, - force=command[15]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_hip_fe, - controlMode=mode, - force=command[16]) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_knee_fe_jd, controlMode=mode, force=command[4] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_ankle_fe, controlMode=mode, force=command[5] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_ankle_ie, controlMode=mode, force=command[6] + ) + + # LH + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_shoulder_fe, controlMode=mode, force=command[7] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_shoulder_aa, controlMode=mode, force=command[8] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_shoulder_ie, controlMode=mode, force=command[9] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_elbow_fe, controlMode=mode, force=command[10] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_wrist_ps, controlMode=mode, force=command[11] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_wrist_pitch, controlMode=mode, force=command[12] + ) + + # neck + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.neck_pitch, controlMode=mode, force=command[13] + ) + + # RF + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_hip_ie, controlMode=mode, force=command[14] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_hip_aa, controlMode=mode, force=command[15] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_hip_fe, controlMode=mode, force=command[16] + ) # pb.setJointMotorControl2(robot, PybulletDracoJointIdx.r_knee_fe_jd, controlMode=mode, force=command[17]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_knee_fe_jd, - controlMode=mode, - force=command[18]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_ankle_fe, - controlMode=mode, - force=command[19]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_ankle_ie, - controlMode=mode, - force=command[20]) - - #RH - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_shoulder_fe, - controlMode=mode, - force=command[21]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_shoulder_aa, - controlMode=mode, - force=command[22]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_shoulder_ie, - controlMode=mode, - force=command[23]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_elbow_fe, - controlMode=mode, - force=command[24]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_wrist_ps, - controlMode=mode, - force=command[25]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_wrist_pitch, - controlMode=mode, - force=command[26]) - - -def apply_position_control_to_pybullet(robot, pos_command, vel_command, kp, - kd): + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_knee_fe_jd, controlMode=mode, force=command[18] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_ankle_fe, controlMode=mode, force=command[19] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_ankle_ie, controlMode=mode, force=command[20] + ) + + # RH + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_shoulder_fe, controlMode=mode, force=command[21] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_shoulder_aa, controlMode=mode, force=command[22] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_shoulder_ie, controlMode=mode, force=command[23] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_elbow_fe, controlMode=mode, force=command[24] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_wrist_ps, controlMode=mode, force=command[25] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_wrist_pitch, controlMode=mode, force=command[26] + ) + + +def apply_position_control_to_pybullet(robot, pos_command, vel_command, kp, kd): mode = pb.POSITION_CONTROL - #LF - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_hip_ie, - controlMode=mode, - targetPosition=pos_command[0], - targetVelocity=vel_command[0], - positionGain=kp[0], - velocityGain=kd[0]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_hip_aa, - controlMode=mode, - targetPosition=pos_command[1], - targetVelocity=vel_command[1], - positionGain=kp[1], - velocityGain=kd[1]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_hip_fe, - controlMode=mode, - targetPosition=pos_command[2], - targetVelocity=vel_command[2], - positionGain=kp[2], - velocityGain=kd[2]) + # LF + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_hip_ie, + controlMode=mode, + targetPosition=pos_command[0], + targetVelocity=vel_command[0], + positionGain=kp[0], + velocityGain=kd[0], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_hip_aa, + controlMode=mode, + targetPosition=pos_command[1], + targetVelocity=vel_command[1], + positionGain=kp[1], + velocityGain=kd[1], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_hip_fe, + controlMode=mode, + targetPosition=pos_command[2], + targetVelocity=vel_command[2], + positionGain=kp[2], + velocityGain=kd[2], + ) # pb.setJointMotorControl2(robot, PybulletDracoJointIdx.l_knee_fe_jp, controlMode=mode, targetPosition=pos_command[3]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_knee_fe_jd, - controlMode=mode, - targetPosition=pos_command[4], - targetVelocity=vel_command[4], - positionGain=kp[4], - velocityGain=kd[4]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_ankle_fe, - controlMode=mode, - targetPosition=pos_command[5], - targetVelocity=vel_command[5], - positionGain=kp[5], - velocityGain=kd[5]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_ankle_ie, - controlMode=mode, - targetPosition=pos_command[6], - targetVelocity=vel_command[6], - positionGain=kp[6], - velocityGain=kd[6]) - - #LH - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_shoulder_fe, - controlMode=mode, - targetPosition=pos_command[7], - targetVelocity=vel_command[7], - positionGain=kp[7], - velocityGain=kd[7]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_shoulder_aa, - controlMode=mode, - targetPosition=pos_command[8], - targetVelocity=vel_command[8], - positionGain=kp[8], - velocityGain=kd[8]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_shoulder_ie, - controlMode=mode, - targetPosition=pos_command[9], - targetVelocity=vel_command[9], - positionGain=kp[9], - velocityGain=kd[9]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_elbow_fe, - controlMode=mode, - targetPosition=pos_command[10], - targetVelocity=vel_command[10], - positionGain=kp[10], - velocityGain=kd[10]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_wrist_ps, - controlMode=mode, - targetPosition=pos_command[11], - targetVelocity=vel_command[11], - positionGain=kp[11], - velocityGain=kd[11]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_wrist_pitch, - controlMode=mode, - targetPosition=pos_command[12], - targetVelocity=vel_command[12], - positionGain=kp[12], - velocityGain=kd[12]) - - #neck - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.neck_pitch, - controlMode=mode, - targetPosition=pos_command[13], - targetVelocity=vel_command[13], - positionGain=kp[13], - velocityGain=kd[13]) - - #RF - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_hip_ie, - controlMode=mode, - targetPosition=pos_command[14], - targetVelocity=vel_command[14], - positionGain=kp[14], - velocityGain=kd[14]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_hip_aa, - controlMode=mode, - targetPosition=pos_command[15], - targetVelocity=vel_command[15], - positionGain=kp[15], - velocityGain=kd[15]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_hip_fe, - controlMode=mode, - targetPosition=pos_command[16], - targetVelocity=vel_command[16], - positionGain=kp[16], - velocityGain=kd[16]) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_knee_fe_jd, + controlMode=mode, + targetPosition=pos_command[4], + targetVelocity=vel_command[4], + positionGain=kp[4], + velocityGain=kd[4], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_ankle_fe, + controlMode=mode, + targetPosition=pos_command[5], + targetVelocity=vel_command[5], + positionGain=kp[5], + velocityGain=kd[5], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_ankle_ie, + controlMode=mode, + targetPosition=pos_command[6], + targetVelocity=vel_command[6], + positionGain=kp[6], + velocityGain=kd[6], + ) + + # LH + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_shoulder_fe, + controlMode=mode, + targetPosition=pos_command[7], + targetVelocity=vel_command[7], + positionGain=kp[7], + velocityGain=kd[7], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_shoulder_aa, + controlMode=mode, + targetPosition=pos_command[8], + targetVelocity=vel_command[8], + positionGain=kp[8], + velocityGain=kd[8], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_shoulder_ie, + controlMode=mode, + targetPosition=pos_command[9], + targetVelocity=vel_command[9], + positionGain=kp[9], + velocityGain=kd[9], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_elbow_fe, + controlMode=mode, + targetPosition=pos_command[10], + targetVelocity=vel_command[10], + positionGain=kp[10], + velocityGain=kd[10], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_wrist_ps, + controlMode=mode, + targetPosition=pos_command[11], + targetVelocity=vel_command[11], + positionGain=kp[11], + velocityGain=kd[11], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.l_wrist_pitch, + controlMode=mode, + targetPosition=pos_command[12], + targetVelocity=vel_command[12], + positionGain=kp[12], + velocityGain=kd[12], + ) + + # neck + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.neck_pitch, + controlMode=mode, + targetPosition=pos_command[13], + targetVelocity=vel_command[13], + positionGain=kp[13], + velocityGain=kd[13], + ) + + # RF + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_hip_ie, + controlMode=mode, + targetPosition=pos_command[14], + targetVelocity=vel_command[14], + positionGain=kp[14], + velocityGain=kd[14], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_hip_aa, + controlMode=mode, + targetPosition=pos_command[15], + targetVelocity=vel_command[15], + positionGain=kp[15], + velocityGain=kd[15], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_hip_fe, + controlMode=mode, + targetPosition=pos_command[16], + targetVelocity=vel_command[16], + positionGain=kp[16], + velocityGain=kd[16], + ) # pb.setJointMotorControl2(robot, PybulletDracoJointIdx.r_knee_fe_jd, controlMode=mode, targetPosition=pos_command[17]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_knee_fe_jd, - controlMode=mode, - targetPosition=pos_command[18], - targetVelocity=vel_command[18], - positionGain=kp[18], - velocityGain=kd[18]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_ankle_fe, - controlMode=mode, - targetPosition=pos_command[19], - targetVelocity=vel_command[19], - positionGain=kp[19], - velocityGain=kd[19]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_ankle_ie, - controlMode=mode, - targetPosition=pos_command[20], - targetVelocity=vel_command[20], - positionGain=kp[20], - velocityGain=kd[20]) - - #RH - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_shoulder_fe, - controlMode=mode, - targetPosition=pos_command[21], - targetVelocity=vel_command[21], - positionGain=kp[21], - velocityGain=kd[21]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_shoulder_aa, - controlMode=mode, - targetPosition=pos_command[22], - targetVelocity=vel_command[22], - positionGain=kp[22], - velocityGain=kd[22]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_shoulder_ie, - controlMode=mode, - targetPosition=pos_command[23], - targetVelocity=vel_command[23], - positionGain=kp[23], - velocityGain=kd[23]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_elbow_fe, - controlMode=mode, - targetPosition=pos_command[24], - targetVelocity=vel_command[24], - positionGain=kp[24], - velocityGain=kd[24]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_wrist_ps, - controlMode=mode, - targetPosition=pos_command[25], - targetVelocity=vel_command[25], - positionGain=kp[25], - velocityGain=kd[25]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_wrist_pitch, - controlMode=mode, - targetPosition=pos_command[26], - targetVelocity=vel_command[26], - positionGain=kp[26], - velocityGain=kd[26]) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_knee_fe_jd, + controlMode=mode, + targetPosition=pos_command[18], + targetVelocity=vel_command[18], + positionGain=kp[18], + velocityGain=kd[18], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_ankle_fe, + controlMode=mode, + targetPosition=pos_command[19], + targetVelocity=vel_command[19], + positionGain=kp[19], + velocityGain=kd[19], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_ankle_ie, + controlMode=mode, + targetPosition=pos_command[20], + targetVelocity=vel_command[20], + positionGain=kp[20], + velocityGain=kd[20], + ) + + # RH + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_shoulder_fe, + controlMode=mode, + targetPosition=pos_command[21], + targetVelocity=vel_command[21], + positionGain=kp[21], + velocityGain=kd[21], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_shoulder_aa, + controlMode=mode, + targetPosition=pos_command[22], + targetVelocity=vel_command[22], + positionGain=kp[22], + velocityGain=kd[22], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_shoulder_ie, + controlMode=mode, + targetPosition=pos_command[23], + targetVelocity=vel_command[23], + positionGain=kp[23], + velocityGain=kd[23], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_elbow_fe, + controlMode=mode, + targetPosition=pos_command[24], + targetVelocity=vel_command[24], + positionGain=kp[24], + velocityGain=kd[24], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_wrist_ps, + controlMode=mode, + targetPosition=pos_command[25], + targetVelocity=vel_command[25], + positionGain=kp[25], + velocityGain=kd[25], + ) + pb.setJointMotorControl2( + robot, + PybulletDracoJointIdx.r_wrist_pitch, + controlMode=mode, + targetPosition=pos_command[26], + targetVelocity=vel_command[26], + positionGain=kp[26], + velocityGain=kd[26], + ) def set_init_config_pybullet_robot(robot): # Upperbody - pb.resetJointState(robot, PybulletDracoJointIdx.l_shoulder_aa, np.pi / 6, - 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.l_elbow_fe, -np.pi / 2, 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.r_shoulder_aa, -np.pi / 6, - 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.r_elbow_fe, -np.pi / 2, 0.) + pb.resetJointState(robot, PybulletDracoJointIdx.l_shoulder_aa, np.pi / 6, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.l_elbow_fe, -np.pi / 2, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_shoulder_aa, -np.pi / 6, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_elbow_fe, -np.pi / 2, 0.0) # Lowerbody hip_yaw_angle = 0 - pb.resetJointState(robot, PybulletDracoJointIdx.l_hip_aa, - np.radians(hip_yaw_angle), 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.l_hip_fe, -np.pi / 4, 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.l_knee_fe_jp, np.pi / 4, - 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.l_knee_fe_jd, np.pi / 4, - 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.l_ankle_fe, -np.pi / 4, 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.l_ankle_ie, - np.radians(-hip_yaw_angle), 0.) - - pb.resetJointState(robot, PybulletDracoJointIdx.r_hip_aa, - np.radians(-hip_yaw_angle), 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.r_hip_fe, -np.pi / 4, 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.r_knee_fe_jp, np.pi / 4, - 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.r_knee_fe_jd, np.pi / 4, - 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.r_ankle_fe, -np.pi / 4, 0.) - pb.resetJointState(robot, PybulletDracoJointIdx.r_ankle_ie, - np.radians(hip_yaw_angle), 0.) + pb.resetJointState( + robot, PybulletDracoJointIdx.l_hip_aa, np.radians(hip_yaw_angle), 0.0 + ) + pb.resetJointState(robot, PybulletDracoJointIdx.l_hip_fe, -np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.l_knee_fe_jp, np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.l_knee_fe_jd, np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.l_ankle_fe, -np.pi / 4, 0.0) + pb.resetJointState( + robot, PybulletDracoJointIdx.l_ankle_ie, np.radians(-hip_yaw_angle), 0.0 + ) + + pb.resetJointState( + robot, PybulletDracoJointIdx.r_hip_aa, np.radians(-hip_yaw_angle), 0.0 + ) + pb.resetJointState(robot, PybulletDracoJointIdx.r_hip_fe, -np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_knee_fe_jp, np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_knee_fe_jd, np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_ankle_fe, -np.pi / 4, 0.0) + pb.resetJointState( + robot, PybulletDracoJointIdx.r_ankle_ie, np.radians(hip_yaw_angle), 0.0 + ) def signal_handler(signal, frame): # if Config.VIDEO_RECORD: # pybullet_util.make_video(video_dir, False) if Config.MEASURE_COMPUTATION_TIME: - print('========================================================') + print("========================================================") print('saving list of compuation time in "compuation_time.txt"') - print('========================================================') - np.savetxt('computation_time.txt', - np.array([compuation_cal_list]), - delimiter=',') + print("========================================================") + np.savetxt( + "computation_time.txt", np.array([compuation_cal_list]), delimiter="," + ) if Config.VIDEO_RECORD: - print('========================================================') - print('Making Video') - print('========================================================') + print("========================================================") + print("Making Video") + print("========================================================") pybullet_util.make_video(video_dir) pb.disconnect() @@ -523,18 +515,20 @@ def signal_handler(signal, frame): signal.signal(signal.SIGINT, signal_handler) if __name__ == "__main__": - ## connect pybullet sim server pb.connect(pb.GUI) pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0) - pb.resetDebugVisualizerCamera(cameraDistance=1.5, - cameraYaw=120, - cameraPitch=-30, - cameraTargetPosition=[0, 0, 0.5]) + pb.resetDebugVisualizerCamera( + cameraDistance=1.5, + cameraYaw=120, + cameraPitch=-30, + cameraTargetPosition=[0, 0, 0.5], + ) ## sim physics setting - pb.setPhysicsEngineParameter(fixedTimeStep=Config.CONTROLLER_DT, - numSubSteps=Config.N_SUBSTEP) + pb.setPhysicsEngineParameter( + fixedTimeStep=Config.CONTROLLER_DT, numSubSteps=Config.N_SUBSTEP + ) pb.setGravity(0, 0, -9.81) ## robot spawn & initial kinematics and dynamics setting @@ -544,118 +538,139 @@ def signal_handler(signal, frame): cwd + "/robot_model/draco/draco_latest_collisions.urdf", Config.INITIAL_BASE_JOINT_POS, Config.INITIAL_BASE_JOINT_QUAT, - useFixedBase=False) + useFixedBase=False, + ) # draco_humanoid = pb.loadURDF(cwd + # "/robot_model/draco/draco3_big_feet.urdf", # Config.INITIAL_BASE_JOINT_POS, # Config.INITIAL_BASE_JOINT_QUAT, # useFixedBase=0) - ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", - useFixedBase=1) + ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", useFixedBase=1) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) - #TODO:modify this function without dictionary container - n_q, n_v, n_a, joint_id_dict, link_id_dict, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config( - draco_humanoid, Config.INITIAL_BASE_JOINT_POS, - Config.INITIAL_BASE_JOINT_QUAT, Config.PRINT_ROBOT_INFO) + # TODO:modify this function without dictionary container + ( + n_q, + n_v, + n_a, + joint_id_dict, + link_id_dict, + pos_basejoint_to_basecom, + rot_basejoint_to_basecom, + ) = pybullet_util.get_robot_config( + draco_humanoid, + Config.INITIAL_BASE_JOINT_POS, + Config.INITIAL_BASE_JOINT_QUAT, + Config.PRINT_ROBOT_INFO, + ) - #robot initial config setting + # robot initial config setting set_init_config_pybullet_robot(draco_humanoid) - #robot joint and link dynamics setting - #TODO:modify this function without dictionary container + # robot joint and link dynamics setting + # TODO:modify this function without dictionary container pybullet_util.set_joint_friction(draco_humanoid, joint_id_dict, 0) - pybullet_util.set_link_damping(draco_humanoid, link_id_dict, 0., 0.) + pybullet_util.set_link_damping(draco_humanoid, link_id_dict, 0.0, 0.0) ## rolling contact joint constraint - c1 = pb.createConstraint(draco_humanoid, - DracoLinkIdx.l_knee_fe_lp, - draco_humanoid, - DracoLinkIdx.l_knee_fe_ld, - jointType=pb.JOINT_GEAR, - jointAxis=[0, 1, 0], - parentFramePosition=[0, 0, 0], - childFramePosition=[0, 0, 0]) + c1 = pb.createConstraint( + draco_humanoid, + DracoLinkIdx.l_knee_fe_lp, + draco_humanoid, + DracoLinkIdx.l_knee_fe_ld, + jointType=pb.JOINT_GEAR, + jointAxis=[0, 1, 0], + parentFramePosition=[0, 0, 0], + childFramePosition=[0, 0, 0], + ) pb.changeConstraint(c1, gearRatio=-1, maxForce=500, erp=10) - c2 = pb.createConstraint(draco_humanoid, - DracoLinkIdx.r_knee_fe_lp, - draco_humanoid, - DracoLinkIdx.r_knee_fe_ld, - jointType=pb.JOINT_GEAR, - jointAxis=[0, 1, 0], - parentFramePosition=[0, 0, 0], - childFramePosition=[0, 0, 0]) + c2 = pb.createConstraint( + draco_humanoid, + DracoLinkIdx.r_knee_fe_lp, + draco_humanoid, + DracoLinkIdx.r_knee_fe_ld, + jointType=pb.JOINT_GEAR, + jointAxis=[0, 1, 0], + parentFramePosition=[0, 0, 0], + childFramePosition=[0, 0, 0], + ) pb.changeConstraint(c2, gearRatio=-1, maxForce=500, erp=10) - #pnc interface, sensor_data, command class + # pnc interface, sensor_data, command class rpc_draco_interface = draco_interface_py.DracoInterface() rpc_draco_sensor_data = draco_interface_py.DracoSensorData() rpc_draco_command = draco_interface_py.DracoCommand() - #TODO - #active jointidx list in sequence (pinocchio model joint order) + # TODO + # active jointidx list in sequence (pinocchio model joint order) active_joint_idx_list = [] - #default robot kinematics information - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + # default robot kinematics information + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(np.array(base_com_quat)) - rot_world_basejoint = util.quat_to_rot( - np.array(Config.INITIAL_BASE_JOINT_QUAT)) + rot_world_basejoint = util.quat_to_rot(np.array(Config.INITIAL_BASE_JOINT_QUAT)) pos_basejoint_to_basecom = np.dot( rot_world_basejoint.transpose(), - base_com_pos - np.array(Config.INITIAL_BASE_JOINT_POS)) - rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - rot_world_basecom) + base_com_pos - np.array(Config.INITIAL_BASE_JOINT_POS), + ) + rot_basejoint_to_basecom = np.dot( + rot_world_basejoint.transpose(), rot_world_basecom + ) # Run Simulation dt = Config.CONTROLLER_DT count = 0 jpg_count = 0 - ## simulation options if Config.MEASURE_COMPUTATION_TIME: timer = TicToc() compuation_cal_list = [] if Config.VIDEO_RECORD: - video_dir = 'video/draco' + video_dir = "video/draco" if os.path.exists(video_dir): shutil.rmtree(video_dir) os.makedirs(video_dir) - previous_torso_velocity = np.array([0., 0., 0.]) + previous_torso_velocity = np.array([0.0, 0.0, 0.0]) - rate = RateLimiter(frequency=1. / dt) + rate = RateLimiter(frequency=1.0 / dt) perturb = pb.addUserDebugParameter("External Force", -1000, 1000, 0) - while (True): + while True: ############################################################ # Moving Camera Setting ############################################################ base_pos, base_ori = pb.getBasePositionAndOrientation(draco_humanoid) - pb.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=120, cameraPitch=-30, cameraTargetPosition=base_pos + np.array([0.5, 0.3, -base_pos[2] +1])) + pb.resetDebugVisualizerCamera( + cameraDistance=1.5, + cameraYaw=120, + cameraPitch=-30, + cameraTargetPosition=base_pos + np.array([0.5, 0.3, -base_pos[2] + 1]), + ) ############################################################################### - #Debugging Purpose + # Debugging Purpose ############################################################################## ##debugging state estimator by calculating groundtruth basejoint states - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(base_com_quat) - rot_world_basejoint = np.dot(rot_world_basecom, - rot_basejoint_to_basecom.transpose()) - base_joint_pos = base_com_pos - np.dot(rot_world_basejoint, - pos_basejoint_to_basecom) + rot_world_basejoint = np.dot( + rot_world_basecom, rot_basejoint_to_basecom.transpose() + ) + base_joint_pos = base_com_pos - np.dot( + rot_world_basejoint, pos_basejoint_to_basecom + ) base_joint_quat = util.rot_to_quat(rot_world_basejoint) base_com_lin_vel, base_com_ang_vel = pb.getBaseVelocity(draco_humanoid) - trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, - pos_basejoint_to_basecom) + trans_joint_com = liegroup.RpToTrans( + rot_basejoint_to_basecom, pos_basejoint_to_basecom + ) adjoint_joint_com = liegroup.Adjoint(trans_joint_com) twist_basecom_in_world = np.zeros(6) twist_basecom_in_world[0:3] = base_com_ang_vel @@ -663,19 +678,20 @@ def signal_handler(signal, frame): augrot_basecom_world = np.zeros((6, 6)) augrot_basecom_world[0:3, 0:3] = rot_world_basecom.transpose() augrot_basecom_world[3:6, 3:6] = rot_world_basecom.transpose() - twist_basecom_in_basecom = np.dot(augrot_basecom_world, - twist_basecom_in_world) - twist_basejoint_in_basejoint = np.dot(adjoint_joint_com, - twist_basecom_in_basecom) + twist_basecom_in_basecom = np.dot(augrot_basecom_world, twist_basecom_in_world) + twist_basejoint_in_basejoint = np.dot( + adjoint_joint_com, twist_basecom_in_basecom + ) augrot_world_basejoint = np.zeros((6, 6)) augrot_world_basejoint[0:3, 0:3] = rot_world_basejoint augrot_world_basejoint[3:6, 3:6] = rot_world_basejoint - twist_basejoint_in_world = np.dot(augrot_world_basejoint, - twist_basejoint_in_basejoint) + twist_basejoint_in_world = np.dot( + augrot_world_basejoint, twist_basejoint_in_basejoint + ) base_joint_ang_vel = twist_basejoint_in_world[0:3] base_joint_lin_vel = twist_basejoint_in_world[3:6] - #pass debugged data to rpc interface + # pass debugged data to rpc interface rpc_draco_sensor_data.base_joint_pos_ = base_joint_pos rpc_draco_sensor_data.base_joint_quat_ = base_joint_quat rpc_draco_sensor_data.base_joint_lin_vel_ = base_joint_lin_vel @@ -685,44 +701,52 @@ def signal_handler(signal, frame): # Get Keyboard Event keys = pb.getKeyboardEvents() - if pybullet_util.is_key_triggered(keys, '1'): + if pybullet_util.is_key_triggered(keys, "1"): rpc_draco_interface.interrupt_.PressOne() - elif pybullet_util.is_key_triggered(keys, '2'): + elif pybullet_util.is_key_triggered(keys, "2"): rpc_draco_interface.interrupt_.PressTwo() - elif pybullet_util.is_key_triggered(keys, '4'): + elif pybullet_util.is_key_triggered(keys, "4"): rpc_draco_interface.interrupt_.PressFour() - elif pybullet_util.is_key_triggered(keys, '5'): + elif pybullet_util.is_key_triggered(keys, "5"): rpc_draco_interface.interrupt_.PressFive() - elif pybullet_util.is_key_triggered(keys, '6'): + elif pybullet_util.is_key_triggered(keys, "6"): rpc_draco_interface.interrupt_.PressSix() - elif pybullet_util.is_key_triggered(keys, '7'): + elif pybullet_util.is_key_triggered(keys, "7"): rpc_draco_interface.interrupt_.PressSeven() - elif pybullet_util.is_key_triggered(keys, '8'): + elif pybullet_util.is_key_triggered(keys, "8"): rpc_draco_interface.interrupt_.PressEight() - elif pybullet_util.is_key_triggered(keys, '9'): + elif pybullet_util.is_key_triggered(keys, "9"): rpc_draco_interface.interrupt_.PressNine() - elif pybullet_util.is_key_triggered(keys, 'm'): + elif pybullet_util.is_key_triggered(keys, "m"): rpc_draco_interface.interrupt_.PressM() - elif pybullet_util.is_key_triggered(keys, 'x'): + elif pybullet_util.is_key_triggered(keys, "x"): rpc_draco_interface.interrupt_.PressX() - elif pybullet_util.is_key_triggered(keys, 'y'): + elif pybullet_util.is_key_triggered(keys, "y"): rpc_draco_interface.interrupt_.PressY() - elif pybullet_util.is_key_triggered(keys, 'z'): + elif pybullet_util.is_key_triggered(keys, "z"): rpc_draco_interface.interrupt_.PressZ() - elif pybullet_util.is_key_triggered(keys, 'd'): + elif pybullet_util.is_key_triggered(keys, "d"): rpc_draco_interface.interrupt_.PressD() - elif pybullet_util.is_key_triggered(keys, 'p'): + elif pybullet_util.is_key_triggered(keys, "p"): pos = [0.0, 0.0, 0.0] - force = [ 0.0, pb.readUserDebugParameter(perturb), 0.0] - pb.applyExternalForce(draco_humanoid, DracoLinkIdx.torso_com_link, force, pos, - pb.WORLD_FRAME) + force = [0.0, pb.readUserDebugParameter(perturb), 0.0] + pb.applyExternalForce( + draco_humanoid, DracoLinkIdx.torso_com_link, force, pos, pb.WORLD_FRAME + ) print(f"=================force: {force}=================") - #get sensor data - imu_frame_quat, imu_ang_vel, imu_dvel, joint_pos, joint_vel, b_lf_contact, b_rf_contact = get_sensor_data_from_pybullet( - draco_humanoid) - - #copy sensor data to rpc sensor data class + # get sensor data + ( + imu_frame_quat, + imu_ang_vel, + imu_dvel, + joint_pos, + joint_vel, + b_lf_contact, + b_rf_contact, + ) = get_sensor_data_from_pybullet(draco_humanoid) + + # copy sensor data to rpc sensor data class rpc_draco_sensor_data.imu_frame_quat_ = imu_frame_quat rpc_draco_sensor_data.imu_ang_vel_ = imu_ang_vel rpc_draco_sensor_data.imu_dvel_ = imu_dvel @@ -738,19 +762,18 @@ def signal_handler(signal, frame): if Config.MEASURE_COMPUTATION_TIME: timer.tic() - rpc_draco_interface.GetCommand(rpc_draco_sensor_data, - rpc_draco_command) + rpc_draco_interface.GetCommand(rpc_draco_sensor_data, rpc_draco_command) if Config.MEASURE_COMPUTATION_TIME: comp_time = timer.tocvalue() compuation_cal_list.append(comp_time) - #copy command data from rpc command class + # copy command data from rpc command class rpc_trq_command = rpc_draco_command.joint_trq_cmd_ rpc_joint_pos_command = rpc_draco_command.joint_pos_cmd_ rpc_joint_vel_command = rpc_draco_command.joint_vel_cmd_ - #apply command to pybullet robot + # apply command to pybullet robot # joint impedance control # trq_command = rpc_trq_command + JointGains.kp.dot( @@ -772,7 +795,8 @@ def signal_handler(signal, frame): # lfoot_pos = pybullet_util.get_link_iso(draco_humanoid, # save current torso velocity for next iteration previous_torso_velocity = pybullet_util.get_link_vel( - draco_humanoid, link_id_dict['torso_imu'])[3:6] + draco_humanoid, link_id_dict["torso_imu"] + )[3:6] # DracoLinkIdx.l_foot_contact)[0:3, 3] # rfoot_pos = pybullet_util.get_link_iso(draco_humanoid, @@ -790,12 +814,14 @@ def signal_handler(signal, frame): # Save Image file if (Config.VIDEO_RECORD) and (count % Config.RECORD_FREQ == 0): camera_data = pb.getDebugVisualizerCamera() - frame = pybullet_util.get_camera_image_from_debug_camera(camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT) - filename = video_dir + '/step%06d.jpg' % jpg_count + frame = pybullet_util.get_camera_image_from_debug_camera( + camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT + ) + filename = video_dir + "/step%06d.jpg" % jpg_count cv2.imwrite(filename, frame) jpg_count += 1 - pb.stepSimulation() #step simulation + pb.stepSimulation() # step simulation # rate.sleep() # while loop rate limiter count += 1 diff --git a/simulator/pybullet/draco_manipulation_main.py b/simulator/pybullet/draco_manipulation_main.py index 9c439cda..932ffb78 100644 --- a/simulator/pybullet/draco_manipulation_main.py +++ b/simulator/pybullet/draco_manipulation_main.py @@ -1,5 +1,4 @@ import pybullet as pb -import time import os import sys import numpy as np @@ -46,154 +45,111 @@ def get_sensor_data_from_pybullet(robot): joint_pos, joint_vel = np.zeros(27), np.zeros(27) imu_frame_quat = np.array( - pb.getLinkState(robot, DracoManipulationLinkIdx.torso_imu, 1, 1)[1]) + pb.getLinkState(robot, DracoManipulationLinkIdx.torso_imu, 1, 1)[1] + ) # LF - joint_pos[0] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_ie)[0] - joint_pos[1] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_aa)[0] - joint_pos[2] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_fe)[0] - joint_pos[3] = pb.getJointState(robot, - DracoManipulationJointIdx.l_knee_fe_jp)[0] - joint_pos[4] = pb.getJointState(robot, - DracoManipulationJointIdx.l_knee_fe_jd)[0] - joint_pos[5] = pb.getJointState(robot, - DracoManipulationJointIdx.l_ankle_fe)[0] - joint_pos[6] = pb.getJointState(robot, - DracoManipulationJointIdx.l_ankle_ie)[0] + joint_pos[0] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_ie)[0] + joint_pos[1] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_aa)[0] + joint_pos[2] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_fe)[0] + joint_pos[3] = pb.getJointState(robot, DracoManipulationJointIdx.l_knee_fe_jp)[0] + joint_pos[4] = pb.getJointState(robot, DracoManipulationJointIdx.l_knee_fe_jd)[0] + joint_pos[5] = pb.getJointState(robot, DracoManipulationJointIdx.l_ankle_fe)[0] + joint_pos[6] = pb.getJointState(robot, DracoManipulationJointIdx.l_ankle_ie)[0] # LH - joint_pos[7] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_fe)[0] - joint_pos[8] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_aa)[0] - joint_pos[9] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_ie)[0] - joint_pos[10] = pb.getJointState(robot, - DracoManipulationJointIdx.l_elbow_fe)[0] - joint_pos[11] = pb.getJointState(robot, - DracoManipulationJointIdx.l_wrist_ps)[0] - joint_pos[12] = pb.getJointState( - robot, DracoManipulationJointIdx.l_wrist_pitch)[0] + joint_pos[7] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_fe)[0] + joint_pos[8] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_aa)[0] + joint_pos[9] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_ie)[0] + joint_pos[10] = pb.getJointState(robot, DracoManipulationJointIdx.l_elbow_fe)[0] + joint_pos[11] = pb.getJointState(robot, DracoManipulationJointIdx.l_wrist_ps)[0] + joint_pos[12] = pb.getJointState(robot, DracoManipulationJointIdx.l_wrist_pitch)[0] # neck - joint_pos[13] = pb.getJointState(robot, - DracoManipulationJointIdx.neck_pitch)[0] + joint_pos[13] = pb.getJointState(robot, DracoManipulationJointIdx.neck_pitch)[0] # RF - joint_pos[14] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_ie)[0] - joint_pos[15] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_aa)[0] - joint_pos[16] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_fe)[0] - joint_pos[17] = pb.getJointState(robot, - DracoManipulationJointIdx.r_knee_fe_jp)[0] - joint_pos[18] = pb.getJointState(robot, - DracoManipulationJointIdx.r_knee_fe_jd)[0] - joint_pos[19] = pb.getJointState(robot, - DracoManipulationJointIdx.r_ankle_fe)[0] - joint_pos[20] = pb.getJointState(robot, - DracoManipulationJointIdx.r_ankle_ie)[0] + joint_pos[14] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_ie)[0] + joint_pos[15] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_aa)[0] + joint_pos[16] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_fe)[0] + joint_pos[17] = pb.getJointState(robot, DracoManipulationJointIdx.r_knee_fe_jp)[0] + joint_pos[18] = pb.getJointState(robot, DracoManipulationJointIdx.r_knee_fe_jd)[0] + joint_pos[19] = pb.getJointState(robot, DracoManipulationJointIdx.r_ankle_fe)[0] + joint_pos[20] = pb.getJointState(robot, DracoManipulationJointIdx.r_ankle_ie)[0] # RH - joint_pos[21] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_fe)[0] - joint_pos[22] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_aa)[0] - joint_pos[23] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_ie)[0] - joint_pos[24] = pb.getJointState(robot, - DracoManipulationJointIdx.r_elbow_fe)[0] - joint_pos[25] = pb.getJointState(robot, - DracoManipulationJointIdx.r_wrist_ps)[0] - joint_pos[26] = pb.getJointState( - robot, DracoManipulationJointIdx.r_wrist_pitch)[0] + joint_pos[21] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_fe)[0] + joint_pos[22] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_aa)[0] + joint_pos[23] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_ie)[0] + joint_pos[24] = pb.getJointState(robot, DracoManipulationJointIdx.r_elbow_fe)[0] + joint_pos[25] = pb.getJointState(robot, DracoManipulationJointIdx.r_wrist_ps)[0] + joint_pos[26] = pb.getJointState(robot, DracoManipulationJointIdx.r_wrist_pitch)[0] imu_ang_vel = np.array( - pb.getLinkState(robot, DracoManipulationLinkIdx.torso_imu, 1, 1)[7]) + pb.getLinkState(robot, DracoManipulationLinkIdx.torso_imu, 1, 1)[7] + ) - imu_dvel = pybullet_util.simulate_dVel_data(robot, link_id_dict, - previous_torso_velocity) + imu_dvel = pybullet_util.simulate_dVel_data( + robot, link_id_dict, previous_torso_velocity + ) # LF - joint_vel[0] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_ie)[1] - joint_vel[1] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_aa)[1] - joint_vel[2] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_fe)[1] - joint_vel[3] = pb.getJointState(robot, - DracoManipulationJointIdx.l_knee_fe_jp)[1] - joint_vel[4] = pb.getJointState(robot, - DracoManipulationJointIdx.l_knee_fe_jd)[1] - joint_vel[5] = pb.getJointState(robot, - DracoManipulationJointIdx.l_ankle_fe)[1] - joint_vel[6] = pb.getJointState(robot, - DracoManipulationJointIdx.l_ankle_ie)[1] + joint_vel[0] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_ie)[1] + joint_vel[1] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_aa)[1] + joint_vel[2] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_fe)[1] + joint_vel[3] = pb.getJointState(robot, DracoManipulationJointIdx.l_knee_fe_jp)[1] + joint_vel[4] = pb.getJointState(robot, DracoManipulationJointIdx.l_knee_fe_jd)[1] + joint_vel[5] = pb.getJointState(robot, DracoManipulationJointIdx.l_ankle_fe)[1] + joint_vel[6] = pb.getJointState(robot, DracoManipulationJointIdx.l_ankle_ie)[1] # LH - joint_vel[7] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_fe)[1] - joint_vel[8] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_aa)[1] - joint_vel[9] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_ie)[1] - joint_vel[10] = pb.getJointState(robot, - DracoManipulationJointIdx.l_elbow_fe)[1] - joint_vel[11] = pb.getJointState(robot, - DracoManipulationJointIdx.l_wrist_ps)[1] - joint_vel[12] = pb.getJointState( - robot, DracoManipulationJointIdx.l_wrist_pitch)[1] + joint_vel[7] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_fe)[1] + joint_vel[8] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_aa)[1] + joint_vel[9] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_ie)[1] + joint_vel[10] = pb.getJointState(robot, DracoManipulationJointIdx.l_elbow_fe)[1] + joint_vel[11] = pb.getJointState(robot, DracoManipulationJointIdx.l_wrist_ps)[1] + joint_vel[12] = pb.getJointState(robot, DracoManipulationJointIdx.l_wrist_pitch)[1] # neck - joint_vel[13] = pb.getJointState(robot, - DracoManipulationJointIdx.neck_pitch)[1] + joint_vel[13] = pb.getJointState(robot, DracoManipulationJointIdx.neck_pitch)[1] # RF - joint_vel[14] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_ie)[1] - joint_vel[15] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_aa)[1] - joint_vel[16] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_fe)[1] - joint_vel[17] = pb.getJointState(robot, - DracoManipulationJointIdx.r_knee_fe_jp)[1] - joint_vel[18] = pb.getJointState(robot, - DracoManipulationJointIdx.r_knee_fe_jd)[1] - joint_vel[19] = pb.getJointState(robot, - DracoManipulationJointIdx.r_ankle_fe)[1] - joint_vel[20] = pb.getJointState(robot, - DracoManipulationJointIdx.r_ankle_ie)[1] + joint_vel[14] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_ie)[1] + joint_vel[15] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_aa)[1] + joint_vel[16] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_fe)[1] + joint_vel[17] = pb.getJointState(robot, DracoManipulationJointIdx.r_knee_fe_jp)[1] + joint_vel[18] = pb.getJointState(robot, DracoManipulationJointIdx.r_knee_fe_jd)[1] + joint_vel[19] = pb.getJointState(robot, DracoManipulationJointIdx.r_ankle_fe)[1] + joint_vel[20] = pb.getJointState(robot, DracoManipulationJointIdx.r_ankle_ie)[1] # RH - joint_vel[21] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_fe)[1] - joint_vel[22] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_aa)[1] - joint_vel[23] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_ie)[1] - joint_vel[24] = pb.getJointState(robot, - DracoManipulationJointIdx.r_elbow_fe)[1] - joint_vel[25] = pb.getJointState(robot, - DracoManipulationJointIdx.r_wrist_ps)[1] - joint_vel[26] = pb.getJointState( - robot, DracoManipulationJointIdx.r_wrist_pitch)[1] + joint_vel[21] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_fe)[1] + joint_vel[22] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_aa)[1] + joint_vel[23] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_ie)[1] + joint_vel[24] = pb.getJointState(robot, DracoManipulationJointIdx.r_elbow_fe)[1] + joint_vel[25] = pb.getJointState(robot, DracoManipulationJointIdx.r_wrist_ps)[1] + joint_vel[26] = pb.getJointState(robot, DracoManipulationJointIdx.r_wrist_pitch)[1] # normal force measured on each foot _l_normal_force = 0 contacts = pb.getContactPoints( - bodyA=robot, linkIndexA=DracoManipulationLinkIdx.l_ankle_ie_link) + bodyA=robot, linkIndexA=DracoManipulationLinkIdx.l_ankle_ie_link + ) for contact in contacts: # add z-component on all points of contact _l_normal_force += contact[9] _r_normal_force = 0 contacts = pb.getContactPoints( - bodyA=robot, linkIndexA=DracoManipulationLinkIdx.r_ankle_ie_link) + bodyA=robot, linkIndexA=DracoManipulationLinkIdx.r_ankle_ie_link + ) for contact in contacts: # add z-component on all points of contact _r_normal_force += contact[9] - b_lf_contact = (True if pb.getLinkState( - robot, DracoManipulationLinkIdx.l_foot_contact, 1, 1)[0][2] <= 0.05 - else False) - b_rf_contact = (True if pb.getLinkState( - robot, DracoManipulationLinkIdx.r_foot_contact, 1, 1)[0][2] <= 0.05 - else False) + b_lf_contact = ( + True + if pb.getLinkState(robot, DracoManipulationLinkIdx.l_foot_contact, 1, 1)[0][2] + <= 0.05 + else False + ) + b_rf_contact = ( + True + if pb.getLinkState(robot, DracoManipulationLinkIdx.r_foot_contact, 1, 1)[0][2] + <= 0.05 + else False + ) return ( imu_frame_quat, imu_ang_vel, @@ -212,156 +168,153 @@ def apply_control_input_to_pybullet(robot, command): mode = pb.TORQUE_CONTROL # LF - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_hip_ie, - controlMode=mode, - force=command[0]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_hip_aa, - controlMode=mode, - force=command[1]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_hip_fe, - controlMode=mode, - force=command[2]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_hip_ie, controlMode=mode, force=command[0] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_hip_aa, controlMode=mode, force=command[1] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_hip_fe, controlMode=mode, force=command[2] + ) # pb.setJointMotorControl2(robot, DracoManipulationJointIdx.l_knee_fe_jp, controlMode=mode, force=command[3]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_knee_fe_jd, - controlMode=mode, - force=command[4]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_ankle_fe, - controlMode=mode, - force=command[5]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_ankle_ie, - controlMode=mode, - force=command[6]) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.l_knee_fe_jd, + controlMode=mode, + force=command[4], + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_ankle_fe, controlMode=mode, force=command[5] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_ankle_ie, controlMode=mode, force=command[6] + ) # LH - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_shoulder_fe, - controlMode=mode, - force=command[7]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_shoulder_aa, - controlMode=mode, - force=command[8]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_shoulder_ie, - controlMode=mode, - force=command[9]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_elbow_fe, - controlMode=mode, - force=command[10]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_wrist_ps, - controlMode=mode, - force=command[11]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_wrist_pitch, - controlMode=mode, - force=command[12]) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.l_shoulder_fe, + controlMode=mode, + force=command[7], + ) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.l_shoulder_aa, + controlMode=mode, + force=command[8], + ) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.l_shoulder_ie, + controlMode=mode, + force=command[9], + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_elbow_fe, controlMode=mode, force=command[10] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_wrist_ps, controlMode=mode, force=command[11] + ) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.l_wrist_pitch, + controlMode=mode, + force=command[12], + ) # neck - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.neck_pitch, - controlMode=mode, - force=command[13]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.neck_pitch, controlMode=mode, force=command[13] + ) # RF - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_hip_ie, - controlMode=mode, - force=command[14]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_hip_aa, - controlMode=mode, - force=command[15]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_hip_fe, - controlMode=mode, - force=command[16]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_hip_ie, controlMode=mode, force=command[14] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_hip_aa, controlMode=mode, force=command[15] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_hip_fe, controlMode=mode, force=command[16] + ) # pb.setJointMotorControl2(robot, DracoManipulationJointIdx.r_knee_fe_jd, controlMode=mode, force=command[17]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_knee_fe_jd, - controlMode=mode, - force=command[18]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_ankle_fe, - controlMode=mode, - force=command[19]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_ankle_ie, - controlMode=mode, - force=command[20]) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.r_knee_fe_jd, + controlMode=mode, + force=command[18], + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_ankle_fe, controlMode=mode, force=command[19] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_ankle_ie, controlMode=mode, force=command[20] + ) # RH - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_shoulder_fe, - controlMode=mode, - force=command[21]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_shoulder_aa, - controlMode=mode, - force=command[22]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_shoulder_ie, - controlMode=mode, - force=command[23]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_elbow_fe, - controlMode=mode, - force=command[24]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_wrist_ps, - controlMode=mode, - force=command[25]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_wrist_pitch, - controlMode=mode, - force=command[26]) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.r_shoulder_fe, + controlMode=mode, + force=command[21], + ) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.r_shoulder_aa, + controlMode=mode, + force=command[22], + ) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.r_shoulder_ie, + controlMode=mode, + force=command[23], + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_elbow_fe, controlMode=mode, force=command[24] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_wrist_ps, controlMode=mode, force=command[25] + ) + pb.setJointMotorControl2( + robot, + DracoManipulationJointIdx.r_wrist_pitch, + controlMode=mode, + force=command[26], + ) def set_init_config_pybullet_robot(robot): # Upperbody - pb.resetJointState(robot, DracoManipulationJointIdx.l_shoulder_aa, - np.pi / 6, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_elbow_fe, -np.pi / 2, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_shoulder_aa, - -np.pi / 6, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_elbow_fe, -np.pi / 2, - 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_shoulder_aa, np.pi / 6, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_elbow_fe, -np.pi / 2, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_shoulder_aa, -np.pi / 6, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_elbow_fe, -np.pi / 2, 0.0) # Lowerbody hip_yaw_angle = 0 - pb.resetJointState(robot, DracoManipulationJointIdx.l_hip_aa, - np.radians(hip_yaw_angle), 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_hip_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_knee_fe_jp, - np.pi / 4, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_knee_fe_jd, - np.pi / 4, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_ankle_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_ankle_ie, - np.radians(-hip_yaw_angle), 0.0) - - pb.resetJointState(robot, DracoManipulationJointIdx.r_hip_aa, - np.radians(-hip_yaw_angle), 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_hip_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_knee_fe_jp, - np.pi / 4, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_knee_fe_jd, - np.pi / 4, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_ankle_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_ankle_ie, - np.radians(hip_yaw_angle), 0.0) + pb.resetJointState( + robot, DracoManipulationJointIdx.l_hip_aa, np.radians(hip_yaw_angle), 0.0 + ) + pb.resetJointState(robot, DracoManipulationJointIdx.l_hip_fe, -np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_knee_fe_jp, np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_knee_fe_jd, np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_ankle_fe, -np.pi / 4, 0.0) + pb.resetJointState( + robot, DracoManipulationJointIdx.l_ankle_ie, np.radians(-hip_yaw_angle), 0.0 + ) + + pb.resetJointState( + robot, DracoManipulationJointIdx.r_hip_aa, np.radians(-hip_yaw_angle), 0.0 + ) + pb.resetJointState(robot, DracoManipulationJointIdx.r_hip_fe, -np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_knee_fe_jp, np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_knee_fe_jd, np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_ankle_fe, -np.pi / 4, 0.0) + pb.resetJointState( + robot, DracoManipulationJointIdx.r_ankle_ie, np.radians(hip_yaw_angle), 0.0 + ) def signal_handler(signal, frame): @@ -371,9 +324,9 @@ def signal_handler(signal, frame): print("========================================================") print('saving list of compuation time in "compuation_time.txt"') print("========================================================") - np.savetxt("computation_time.txt", - np.array([compuation_cal_list]), - delimiter=",") + np.savetxt( + "computation_time.txt", np.array([compuation_cal_list]), delimiter="," + ) if Config.VIDEO_RECORD: print("========================================================") @@ -399,8 +352,9 @@ def signal_handler(signal, frame): cameraTargetPosition=[0, 0, 0.5], ) ## sim physics setting - pb.setPhysicsEngineParameter(fixedTimeStep=Config.CONTROLLER_DT, - numSubSteps=Config.N_SUBSTEP) + pb.setPhysicsEngineParameter( + fixedTimeStep=Config.CONTROLLER_DT, numSubSteps=Config.N_SUBSTEP + ) pb.setGravity(0, 0, -9.81) ## robot spawn & initial kinematics and dynamics setting @@ -416,23 +370,30 @@ def signal_handler(signal, frame): useFixedBase=0, ) - ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", - useFixedBase=1) + ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", useFixedBase=1) xOffset = 0.7 - pb.loadURDF(cwd + "/robot_model/bookcase/bookshelf.urdf", - useFixedBase=1, - basePosition=[0 + xOffset, 0, 0.025], - baseOrientation=[0, 0, 0.7068252, 0.7068252]) - pb.loadURDF(cwd + "/robot_model/bookcase/red_can.urdf", - useFixedBase=0, - basePosition=[0 + xOffset, 0.75, 1.05]) - green_can = pb.loadURDF(cwd + "/robot_model/bookcase/green_can.urdf", - useFixedBase=0, - basePosition=[xOffset - 0.0, -0.5, 1.35]) - blue_can = pb.loadURDF(cwd + "/robot_model/bookcase/blue_can.urdf", - useFixedBase=0, - basePosition=[xOffset - 0.05, 0.2, 0.3]) + pb.loadURDF( + cwd + "/robot_model/bookcase/bookshelf.urdf", + useFixedBase=1, + basePosition=[0 + xOffset, 0, 0.025], + baseOrientation=[0, 0, 0.7068252, 0.7068252], + ) + pb.loadURDF( + cwd + "/robot_model/bookcase/red_can.urdf", + useFixedBase=0, + basePosition=[0 + xOffset, 0.75, 1.05], + ) + green_can = pb.loadURDF( + cwd + "/robot_model/bookcase/green_can.urdf", + useFixedBase=0, + basePosition=[xOffset - 0.0, -0.5, 1.35], + ) + blue_can = pb.loadURDF( + cwd + "/robot_model/bookcase/blue_can.urdf", + useFixedBase=0, + basePosition=[xOffset - 0.05, 0.2, 0.3], + ) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) # TODO:modify this function without dictionary container @@ -495,18 +456,17 @@ def signal_handler(signal, frame): active_jointidx_list = [] # for setjointmotorcontrolarray # default robot kinematics information - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(np.array(base_com_quat)) - rot_world_basejoint = util.quat_to_rot( - np.array(Config.INITIAL_BASE_JOINT_QUAT)) + rot_world_basejoint = util.quat_to_rot(np.array(Config.INITIAL_BASE_JOINT_QUAT)) pos_basejoint_to_basecom = np.dot( rot_world_basejoint.transpose(), base_com_pos - np.array(Config.INITIAL_BASE_JOINT_POS), ) - rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - rot_world_basecom) + rot_basejoint_to_basecom = np.dot( + rot_world_basejoint.transpose(), rot_world_basecom + ) # Run Simulation dt = Config.CONTROLLER_DT @@ -553,18 +513,20 @@ def signal_handler(signal, frame): # Debugging Purpose ############################################################################## ##debugging state estimator by calculating groundtruth basejoint states - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(base_com_quat) - rot_world_basejoint = np.dot(rot_world_basecom, - rot_basejoint_to_basecom.transpose()) - base_joint_pos = base_com_pos - np.dot(rot_world_basejoint, - pos_basejoint_to_basecom) + rot_world_basejoint = np.dot( + rot_world_basecom, rot_basejoint_to_basecom.transpose() + ) + base_joint_pos = base_com_pos - np.dot( + rot_world_basejoint, pos_basejoint_to_basecom + ) base_joint_quat = util.rot_to_quat(rot_world_basejoint) base_com_lin_vel, base_com_ang_vel = pb.getBaseVelocity(draco_humanoid) - trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, - pos_basejoint_to_basecom) + trans_joint_com = liegroup.RpToTrans( + rot_basejoint_to_basecom, pos_basejoint_to_basecom + ) adjoint_joint_com = liegroup.Adjoint(trans_joint_com) twist_basecom_in_world = np.zeros(6) twist_basecom_in_world[0:3] = base_com_ang_vel @@ -572,15 +534,16 @@ def signal_handler(signal, frame): augrot_basecom_world = np.zeros((6, 6)) augrot_basecom_world[0:3, 0:3] = rot_world_basecom.transpose() augrot_basecom_world[3:6, 3:6] = rot_world_basecom.transpose() - twist_basecom_in_basecom = np.dot(augrot_basecom_world, - twist_basecom_in_world) - twist_basejoint_in_basejoint = np.dot(adjoint_joint_com, - twist_basecom_in_basecom) + twist_basecom_in_basecom = np.dot(augrot_basecom_world, twist_basecom_in_world) + twist_basejoint_in_basejoint = np.dot( + adjoint_joint_com, twist_basecom_in_basecom + ) augrot_world_basejoint = np.zeros((6, 6)) augrot_world_basejoint[0:3, 0:3] = rot_world_basejoint augrot_world_basejoint[3:6, 3:6] = rot_world_basejoint - twist_basejoint_in_world = np.dot(augrot_world_basejoint, - twist_basejoint_in_basejoint) + twist_basejoint_in_world = np.dot( + augrot_world_basejoint, twist_basejoint_in_basejoint + ) base_joint_ang_vel = twist_basejoint_in_world[0:3] base_joint_lin_vel = twist_basejoint_in_world[3:6] @@ -634,12 +597,13 @@ def signal_handler(signal, frame): l_normal_force = pybullet_util.simulate_contact_sensor(l_normal_force) r_normal_force = pybullet_util.simulate_contact_sensor(r_normal_force) imu_dvel = pybullet_util.add_sensor_noise(imu_dvel, imu_dvel_bias) - imu_ang_vel = pybullet_util.add_sensor_noise(imu_ang_vel, - imu_ang_vel_noise) + imu_ang_vel = pybullet_util.add_sensor_noise(imu_ang_vel, imu_ang_vel_noise) l_normal_force = pybullet_util.add_sensor_noise( - l_normal_force, l_normal_volt_noise) + l_normal_force, l_normal_volt_noise + ) r_normal_force = pybullet_util.add_sensor_noise( - r_normal_force, r_normal_volt_noise) + r_normal_force, r_normal_volt_noise + ) # copy sensor data to rpc sensor data class rpc_draco_sensor_data.imu_frame_quat_ = imu_frame_quat @@ -659,8 +623,7 @@ def signal_handler(signal, frame): if Config.MEASURE_COMPUTATION_TIME: timer.tic() - rpc_draco_interface.GetCommand(rpc_draco_sensor_data, - rpc_draco_command) + rpc_draco_interface.GetCommand(rpc_draco_sensor_data, rpc_draco_command) if Config.MEASURE_COMPUTATION_TIME: comp_time = timer.tocvalue() @@ -677,7 +640,8 @@ def signal_handler(signal, frame): # lfoot_pos = pybullet_util.get_link_iso(draco_humanoid, # save current torso velocity for next iteration previous_torso_velocity = pybullet_util.get_link_vel( - draco_humanoid, link_id_dict["torso_imu"])[3:6] + draco_humanoid, link_id_dict["torso_imu"] + )[3:6] # DracoManipulationLinkIdx.l_foot_contact)[0:3, 3] # rfoot_pos = pybullet_util.get_link_iso(draco_humanoid, @@ -696,7 +660,8 @@ def signal_handler(signal, frame): if (Config.VIDEO_RECORD) and (count % Config.RECORD_FREQ == 0): camera_data = pb.getDebugVisualizerCamera() frame = pybullet_util.get_camera_image_from_debug_camera( - camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT) + camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT + ) filename = video_dir + "/step%06d.jpg" % jpg_count cv2.imwrite(filename, frame) jpg_count += 1 diff --git a/simulator/pybullet/fixed_draco_main.py b/simulator/pybullet/fixed_draco_main.py index 12669de3..184450a1 100644 --- a/simulator/pybullet/fixed_draco_main.py +++ b/simulator/pybullet/fixed_draco_main.py @@ -5,7 +5,6 @@ import numpy as np np.set_printoptions(precision=4) -import ipdb from config.fixed_draco.pybullet.pybullet_params import Config from util.python_utils import pybullet_util @@ -27,34 +26,30 @@ def set_initial_config(robot, joint_id): # Lowerbody hip_yaw_angle = 5 - pb.resetJointState(robot, joint_id["l_hip_aa"], np.radians(hip_yaw_angle), - 0.0) + pb.resetJointState(robot, joint_id["l_hip_aa"], np.radians(hip_yaw_angle), 0.0) pb.resetJointState(robot, joint_id["l_hip_fe"], -np.pi / 4, 0.0) pb.resetJointState(robot, joint_id["l_knee_fe_jp"], np.pi / 4, 0.0) pb.resetJointState(robot, joint_id["l_knee_fe_jd"], np.pi / 4, 0.0) pb.resetJointState(robot, joint_id["l_ankle_fe"], -np.pi / 4, 0.0) - pb.resetJointState(robot, joint_id["l_ankle_ie"], - np.radians(-hip_yaw_angle), 0.0) + pb.resetJointState(robot, joint_id["l_ankle_ie"], np.radians(-hip_yaw_angle), 0.0) - pb.resetJointState(robot, joint_id["r_hip_aa"], np.radians(-hip_yaw_angle), - 0.0) + pb.resetJointState(robot, joint_id["r_hip_aa"], np.radians(-hip_yaw_angle), 0.0) pb.resetJointState(robot, joint_id["r_hip_fe"], -np.pi / 4, 0.0) pb.resetJointState(robot, joint_id["r_knee_fe_jp"], np.pi / 4, 0.0) pb.resetJointState(robot, joint_id["r_knee_fe_jd"], np.pi / 4, 0.0) pb.resetJointState(robot, joint_id["r_ankle_fe"], -np.pi / 4, 0.0) - pb.resetJointState(robot, joint_id["r_ankle_ie"], - np.radians(hip_yaw_angle), 0.0) + pb.resetJointState(robot, joint_id["r_ankle_ie"], np.radians(hip_yaw_angle), 0.0) pb.connect(pb.GUI) -pb.resetDebugVisualizerCamera(cameraDistance=1.5, - cameraYaw=90, - cameraPitch=0, - cameraTargetPosition=[0, 0, 0.8]) +pb.resetDebugVisualizerCamera( + cameraDistance=1.5, cameraYaw=90, cameraPitch=0, cameraTargetPosition=[0, 0, 0.8] +) ## sim physics setting -pb.setPhysicsEngineParameter(fixedTimeStep=Config.CONTROLLER_DT, - numSubSteps=Config.N_SUBSTEP) +pb.setPhysicsEngineParameter( + fixedTimeStep=Config.CONTROLLER_DT, numSubSteps=Config.N_SUBSTEP +) pb.setGravity(0, 0, -9.81) ## robot spawn & initial kinematics and dynamics setting @@ -74,7 +69,8 @@ def set_initial_config(robot, joint_id): Config.INITIAL_BASE_JOINT_POS, Config.INITIAL_BASE_JOINT_QUAT, Config.PRINT_ROBOT_INFO, - )) + ) +) # robot initial config setting set_initial_config(fixed_draco, joint_id) @@ -119,8 +115,8 @@ def set_initial_config(robot, joint_id): count = 0 pybullet_nominal_sensor_data_dict = pybullet_util.get_sensor_data( - fixed_draco, joint_id, link_id, pos_basejoint_to_basecom, - rot_basejoint_to_basecom) + fixed_draco, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom +) ##TEST for plot while True: @@ -134,42 +130,46 @@ def set_initial_config(robot, joint_id): ) pybullet_sensor_data_dict["imu_frame_iso"] = pybullet_util.get_link_iso( - fixed_draco, link_id["torso_imu"]) + fixed_draco, link_id["torso_imu"] + ) pybullet_sensor_data_dict["imu_frame_vel"] = pybullet_util.get_link_vel( - fixed_draco, link_id["torso_imu"]) + fixed_draco, link_id["torso_imu"] + ) base_com_pos_ = pybullet_sensor_data_dict["base_com_pos"] base_com_quat_ = pybullet_sensor_data_dict["base_com_quat"] base_joint_pos_ = pybullet_sensor_data_dict["base_joint_pos"] base_joint_quat_ = pybullet_sensor_data_dict["base_joint_quat"] ##copy pybullet sensor data to pnc sensor data - pnc_draco_sensor_data.joint_positions_ = pybullet_sensor_data_dict[ - "joint_pos"] - pnc_draco_sensor_data.joint_velocities_ = pybullet_sensor_data_dict[ - "joint_vel"] + pnc_draco_sensor_data.joint_positions_ = pybullet_sensor_data_dict["joint_pos"] + pnc_draco_sensor_data.joint_velocities_ = pybullet_sensor_data_dict["joint_vel"] pnc_draco_sensor_data.imu_frame_isometry_ = pybullet_sensor_data_dict[ - "imu_frame_iso"] + "imu_frame_iso" + ] pnc_draco_sensor_data.imu_frame_velocities_ = pybullet_sensor_data_dict[ - "imu_frame_vel"] + "imu_frame_vel" + ] ##debugging - pnc_draco_sensor_data.base_com_pos_ = pybullet_sensor_data_dict[ - "base_com_pos"] - pnc_draco_sensor_data.base_com_quat_ = pybullet_sensor_data_dict[ - "base_com_quat"] + pnc_draco_sensor_data.base_com_pos_ = pybullet_sensor_data_dict["base_com_pos"] + pnc_draco_sensor_data.base_com_quat_ = pybullet_sensor_data_dict["base_com_quat"] pnc_draco_sensor_data.base_com_lin_vel_ = pybullet_sensor_data_dict[ - "base_com_lin_vel"] + "base_com_lin_vel" + ] pnc_draco_sensor_data.base_com_ang_vel_ = pybullet_sensor_data_dict[ - "base_com_ang_vel"] + "base_com_ang_vel" + ] - pnc_draco_sensor_data.base_joint_pos_ = pybullet_sensor_data_dict[ - "base_joint_pos"] + pnc_draco_sensor_data.base_joint_pos_ = pybullet_sensor_data_dict["base_joint_pos"] pnc_draco_sensor_data.base_joint_quat_ = pybullet_sensor_data_dict[ - "base_joint_quat"] + "base_joint_quat" + ] pnc_draco_sensor_data.base_joint_lin_vel_ = pybullet_sensor_data_dict[ - "base_joint_lin_vel"] + "base_joint_lin_vel" + ] pnc_draco_sensor_data.base_joint_ang_vel_ = pybullet_sensor_data_dict[ - "base_joint_ang_vel"] + "base_joint_ang_vel" + ] ##Debugging purpose # print("==================") @@ -198,12 +198,11 @@ def set_initial_config(robot, joint_id): pnc_draco_interface.GetCommand(pnc_draco_sensor_data, pnc_draco_command) ##copy command - pybullet_joint_positions_cmd = copy.deepcopy( - pnc_draco_command.joint_positions_cmd_) + pybullet_joint_positions_cmd = copy.deepcopy(pnc_draco_command.joint_positions_cmd_) pybullet_joint_velocities_cmd = copy.deepcopy( - pnc_draco_command.joint_velocities_cmd_) - pybullet_joint_torques_cmd = copy.deepcopy( - pnc_draco_command.joint_torques_cmd_) + pnc_draco_command.joint_velocities_cmd_ + ) + pybullet_joint_torques_cmd = copy.deepcopy(pnc_draco_command.joint_torques_cmd_) ##delete passive joint command del pybullet_joint_positions_cmd["l_knee_fe_jp"] @@ -221,8 +220,9 @@ def set_initial_config(robot, joint_id): ##apply motor torque # pybullet_util.set_motor_trq(fixed_draco, joint_id, # pybullet_joint_torques_cmd) - pybullet_util.set_motor_impedance(fixed_draco, joint_id, command_dict, - Config.KP, Config.KD) + pybullet_util.set_motor_impedance( + fixed_draco, joint_id, command_dict, Config.KP, Config.KD + ) ## initial pos control # pos_cmd_dict = copy.deepcopy( diff --git a/simulator/pybullet/go2_main.py b/simulator/pybullet/go2_main.py index 8ed72dc1..67dfcf26 100644 --- a/simulator/pybullet/go2_main.py +++ b/simulator/pybullet/go2_main.py @@ -1,20 +1,11 @@ import pybullet as pb -import time import os import sys -import numpy as np cwd = os.getcwd() sys.path.append(cwd) sys.path.append(cwd + "/build/lib") # include pybind module -from util.python_utils import pybullet_util -from util.python_utils import util -from util.python_utils import liegroup - -import signal -import shutil -import cv2 if __name__ == "__main__": ## connect pybullet sim server @@ -36,13 +27,12 @@ robot = pb.loadURDF( cwd + "/robot_model/go2/go2_description.urdf", - [0., 0., 0.45], + [0.0, 0.0, 0.45], [0, 0, 0, 1], useFixedBase=1, ) - ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", - useFixedBase=1) + ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", useFixedBase=1) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) while True: diff --git a/test/dcm_planner_test.cpp b/test/dcm_planner_test.cpp index d54dfd1a..5e4f9e08 100644 --- a/test/dcm_planner_test.cpp +++ b/test/dcm_planner_test.cpp @@ -42,7 +42,8 @@ int main() { bjoint_av, joint_pos, joint_vel, true); // dcm tm initialize - YAML::Node cfg = YAML::LoadFile(THIS_COM "config/draco/sim/pybullet/ihwbc/pnc.yaml"); + YAML::Node cfg = + YAML::LoadFile(THIS_COM "config/draco/sim/pybullet/ihwbc/pnc.yaml"); dcm_tm.InitializeParameters(cfg["dcm_walking"]); Eigen::Vector3d init_dcm = robot.GetRobotComPos(); diff --git a/test/karnopp_compensator_test.cpp b/test/karnopp_compensator_test.cpp index 539f379a..d32a800c 100644 --- a/test/karnopp_compensator_test.cpp +++ b/test/karnopp_compensator_test.cpp @@ -1,5 +1,5 @@ -#include #include "third_party/sciplot/sciplot.hpp" +#include #include "controller/friction_compensator/karnopp_compensator.hpp" diff --git a/util/python_utils/comm.py b/util/python_utils/comm.py index 4c2c4699..abae5957 100644 --- a/util/python_utils/comm.py +++ b/util/python_utils/comm.py @@ -10,13 +10,13 @@ class ZMQServer(object): - def __init__(self, ip, sub_port, pub_port, logger_fn=print, verbose=False): - self.ip = ip self.pub_port = pub_port self.sub_port = sub_port - self._running = Value(ctypes.c_bool, ) + self._running = Value( + ctypes.c_bool, + ) self._init_time = 0.0 self._send_proc = None self._recv_proc = None @@ -41,8 +41,10 @@ def _publish(self): pub_socket.send(msg) if self._verbose: self._logger_fn( - "[{:.2f}] Publishing message...".format(time.time() - - self._init_time)) + "[{:.2f}] Publishing message...".format( + time.time() - self._init_time + ) + ) for key, val in self._cmd.items(): self._logger_fn("{}: {}".format(key, val)) self._cmd.clear() @@ -51,7 +53,6 @@ def _publish(self): self._logger_fn("Publishing Socket is closed...") def _subscribe(self): - context = zmq.Context() sub_socket = context.socket(zmq.SUB) sub_socket.setsockopt(zmq.SUBSCRIBE, b"") @@ -65,8 +66,8 @@ def _subscribe(self): self._obs.update(obs) if self._verbose: self._logger_fn( - "[{:.2f}] Recieved message...".format(time.time() - - self._init_time)) + "[{:.2f}] Recieved message...".format(time.time() - self._init_time) + ) for key, val in self._obs.items(): self._logger_fn("{}: {}".format(key, val)) @@ -74,7 +75,6 @@ def _subscribe(self): self._logger_fn("Subscription Socket is closed...") def start(self): - assert self._send_proc is None and self._recv_proc is None self._cmd.clear() @@ -83,8 +83,12 @@ def start(self): self._running.value = True self._init_time = time.time() - self._send_proc = Process(target=self._publish, ) - self._recv_proc = Process(target=self._subscribe, ) + self._send_proc = Process( + target=self._publish, + ) + self._recv_proc = Process( + target=self._subscribe, + ) self._send_proc.start() self._recv_proc.start() @@ -107,14 +111,14 @@ def obs(self): class ZMQClient(object): - def __init__(self, ip, sub_port, pub_port, logger_fn=print): - self.ip = ip self.pub_port = pub_port self.sub_port = sub_port - self._running = Value(ctypes.c_bool, ) + self._running = Value( + ctypes.c_bool, + ) self._init_time = 0.0 self._send_proc = None self._recv_proc = None @@ -126,7 +130,6 @@ def __init__(self, ip, sub_port, pub_port, logger_fn=print): self._logger_fn("TESTING SPOT LOGGING...") def _publish(self): - context = zmq.Context() pub_socket = context.socket(zmq.PUB) pub_socket.connect("tcp://{}:{}".format(self.ip, self.pub_port)) @@ -139,8 +142,8 @@ def _publish(self): msg = pickle.dumps(obs) pub_socket.send(msg) self._logger_fn( - "[{}] Publishing message...".format(time.time() - - self._init_time)) + "[{}] Publishing message...".format(time.time() - self._init_time) + ) self._logger_fn(self._obs.items()) self._obs.clear() @@ -148,7 +151,6 @@ def _publish(self): self._logger_fn("Publishing Socket is closed...") def _subscribe(self): - context = zmq.Context() sub_socket = context.socket(zmq.SUB) sub_socket.setsockopt(zmq.SUBSCRIBE, b"") @@ -160,15 +162,15 @@ def _subscribe(self): data = sub_socket.recv() cmd = pickle.loads(data) self._cmd.update(cmd) - self._logger_fn("[{}] Recieved message...".format(time.time() - - self._init_time)) + self._logger_fn( + "[{}] Recieved message...".format(time.time() - self._init_time) + ) self._logger_fn(self._cmd.items()) sub_socket.close() self._logger_fn("Subscription Socket is closed...") def start(self): - assert self._send_proc is None and self._recv_proc is None self._obs.clear() @@ -178,8 +180,12 @@ def start(self): self._running.value = True self._init_time = time.time() - self._send_proc = Process(target=self._publish, ) - self._recv_proc = Process(target=self._subscribe, ) + self._send_proc = Process( + target=self._publish, + ) + self._recv_proc = Process( + target=self._subscribe, + ) self._send_proc.start() self._recv_proc.start() diff --git a/util/python_utils/demo.py b/util/python_utils/demo.py index 6f703fd6..71970dd1 100644 --- a/util/python_utils/demo.py +++ b/util/python_utils/demo.py @@ -12,8 +12,7 @@ def post_process_obs(data): return data -def post_process_action(pose_vec_cur, pose_vec_prv, mode='quat'): - +def post_process_action(pose_vec_cur, pose_vec_prv, mode="quat"): pose_cur = np.eye(4) pose_cur[:3, :3] = util.quat_to_rot(pose_vec_cur[3:]) pose_cur[:3, 3] = pose_vec_cur[:3] @@ -25,7 +24,7 @@ def post_process_action(pose_vec_cur, pose_vec_prv, mode='quat'): # Convert to relative pose delta_pose = np.linalg.inv(pose_prv) @ pose_cur delta_pos = delta_pose[:3, 3] - if mode == 'quat': + if mode == "quat": delta_quat = util.rot_to_quat(delta_pose[:3, :3]) else: delta_quat = util.rot_to_euler(delta_pose[:3, :3]) @@ -35,10 +34,9 @@ def post_process_action(pose_vec_cur, pose_vec_prv, mode='quat'): return act_data -def reconstruct_pose(delta_pose_vec, pose_vec_prv, mode='quat'): - +def reconstruct_pose(delta_pose_vec, pose_vec_prv, mode="quat"): delta_pose = np.eye(4) - if mode == 'quat': + if mode == "quat": delta_pose[:3, :3] = util.quat_to_rot(delta_pose_vec[3:]) else: delta_pose[:3, :3] = util.euler_to_rot(delta_pose_vec[3:]) @@ -57,8 +55,7 @@ def reconstruct_pose(delta_pose_vec, pose_vec_prv, mode='quat'): return pose_vec -def post_process_actions(pose_data, mode='quat'): - +def post_process_actions(pose_data, mode="quat"): # Convert to relative pose pose_rot = np.zeros((pose_data.shape[0], 4, 4)) pose_rot[:, 0:3, 0:3] = util.quat_to_rot(pose_data[:, 3:]) @@ -81,12 +78,10 @@ def post_process_actions(pose_data, mode='quat'): delta_quat.append(np.array([0, 0, 0, 1])) delta_rpy.append(np.array([0, 0, 0])) - if mode == 'quat': - act_data = np.concatenate( - [np.array(delta_pos), np.array(delta_quat)], axis=1) + if mode == "quat": + act_data = np.concatenate([np.array(delta_pos), np.array(delta_quat)], axis=1) else: - act_data = np.concatenate( - [np.array(delta_pos), np.array(delta_rpy)], axis=1) + act_data = np.concatenate([np.array(delta_pos), np.array(delta_rpy)], axis=1) return act_data diff --git a/util/python_utils/device/base.py b/util/python_utils/device/base.py index 2ca5900f..a2073021 100644 --- a/util/python_utils/device/base.py +++ b/util/python_utils/device/base.py @@ -1,4 +1,4 @@ -''' +""" Project: Data collection / Teleoprator @@ -10,36 +10,38 @@ * Copyrighted by Mingyo Seo * Bonston Dynamics AI Institute, The University of Texas at Austin -''' +""" -import time from multiprocessing import Process, Value import ctypes class Sensor(object): - ''' - a receiver's configuration for remote commands - ''' + """ + a receiver's configuration for remote commands + """ def __init__(self, name=""): - self._name = name - self._flag_proc = Value(ctypes.c_bool, ) - self._time_proc = Value(ctypes.c_float, ) + self._flag_proc = Value( + ctypes.c_bool, + ) + self._time_proc = Value( + ctypes.c_float, + ) print("{} created".format(self._name)) def _fn_init(self): - raise NotImplementedError def start(self): - # Start streaming with our callback self._flag_proc.value = True - self._proc = Process(target=self._fn_proc, ) + self._proc = Process( + target=self._fn_proc, + ) self._proc.start() self._fn_init() @@ -47,14 +49,12 @@ def start(self): print("{} start!".format(self._name)) def stop(self): - self._flag_proc.value = False self._proc.terminate() print("{} stop!".format(self._name)) def _fn_proc(self): - raise NotImplementedError @property diff --git a/util/python_utils/device/t265.py b/util/python_utils/device/t265.py index b92c61ee..b7f53c97 100644 --- a/util/python_utils/device/t265.py +++ b/util/python_utils/device/t265.py @@ -1,4 +1,4 @@ -''' +""" Project: Data collection / Teleoprator @@ -10,7 +10,8 @@ * Copyrighted by Mingyo Seo * Bonston Dynamics AI Institute, The University of Texas at Austin -''' +""" + import sys import os @@ -35,12 +36,9 @@ class T265(Sensor): - - def __init__(self, - img_stream=False, - timeout=TIMEOUT, - img_size=(HEIGHT, WIDTH)) -> None: - + def __init__( + self, img_stream=False, timeout=TIMEOUT, img_size=(HEIGHT, WIDTH) + ) -> None: self._left_buffer = Queue() self._right_buffer = Queue() self._left_lock = Lock() @@ -68,13 +66,11 @@ def __init__(self, super().__init__("T265") def _fn_init(self): - # Wait until the first frameset is available while np.sum(self.rot**2) == 0: pass def _fn_proc(self): - # Declare RealSense pipeline, encapsulating the actual device and sensors pipe = rs.pipeline() pipe.start(self._cfg) @@ -82,19 +78,17 @@ def _fn_proc(self): init_time = time.time() while self._flag_proc.value: - cur_time = time.time() - init_time - if cur_time - self._time_proc.value < 1. / FPS: + if cur_time - self._time_proc.value < 1.0 / FPS: continue if cur_time - self._time_proc.value > self._timeout: break - self._time_proc.value += 1. / FPS + self._time_proc.value += 1.0 / FPS frames = pipe.wait_for_frames() pose = frames.get_pose_frame().get_pose_data() if self._img_stream: - f1 = frames.get_fisheye_frame(1).as_video_frame() f2 = frames.get_fisheye_frame(2).as_video_frame() @@ -106,17 +100,28 @@ def _fn_proc(self): self._right_buffer.put(np.asanyarray(f2.get_data())) self._right_lock.release() - self._pos_data[:] = (pose.translation.x, pose.translation.y, - pose.translation.z) - self._rot_data[:] = (pose.rotation.x, pose.rotation.y, - pose.rotation.z, pose.rotation.w) - self._vel_data[:] = (pose.velocity.x, pose.velocity.y, - pose.velocity.z) - self._angvel_data[:] = (pose.angular_velocity.x, - pose.angular_velocity.y, - pose.angular_velocity.z) - self._acc_data[:] = (pose.acceleration.x, pose.acceleration.y, - pose.acceleration.z) + self._pos_data[:] = ( + pose.translation.x, + pose.translation.y, + pose.translation.z, + ) + self._rot_data[:] = ( + pose.rotation.x, + pose.rotation.y, + pose.rotation.z, + pose.rotation.w, + ) + self._vel_data[:] = (pose.velocity.x, pose.velocity.y, pose.velocity.z) + self._angvel_data[:] = ( + pose.angular_velocity.x, + pose.angular_velocity.y, + pose.angular_velocity.z, + ) + self._acc_data[:] = ( + pose.acceleration.x, + pose.acceleration.y, + pose.acceleration.z, + ) pipe.stop() @property diff --git a/util/python_utils/pybullet_util.py b/util/python_utils/pybullet_util.py index 83288b47..116701d6 100644 --- a/util/python_utils/pybullet_util.py +++ b/util/python_utils/pybullet_util.py @@ -14,10 +14,7 @@ np.set_printoptions(precision=5) -def get_robot_config(robot, - initial_pos=None, - initial_quat=None, - b_print_info=False): +def get_robot_config(robot, initial_pos=None, initial_quat=None, b_print_info=False): nq, nv, na, joint_id, link_id = 0, 0, 0, OrderedDict(), OrderedDict() link_id[(pb.getBodyInfo(robot)[0]).decode("utf-8")] = -1 for i in range(pb.getNumJoints(robot)): @@ -34,13 +31,12 @@ def get_robot_config(robot, base_pos, base_quat = pb.getBasePositionAndOrientation(robot) rot_world_com = util.quat_to_rot(base_quat) initial_pos = [0.0, 0.0, 0.0] if initial_pos is None else initial_pos - initial_quat = [0.0, 0.0, 0.0, 1.0 - ] if initial_quat is None else initial_quat + initial_quat = [0.0, 0.0, 0.0, 1.0] if initial_quat is None else initial_quat rot_world_basejoint = util.quat_to_rot(np.array(initial_quat)) - pos_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - base_pos - np.array(initial_pos)) - rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - rot_world_com) + pos_basejoint_to_basecom = np.dot( + rot_world_basejoint.transpose(), base_pos - np.array(initial_pos) + ) + rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), rot_world_com) if b_print_info: print("=" * 80) @@ -76,8 +72,9 @@ def get_robot_config(robot, ) -def get_kinematics_config(robot, joint_id, link_id, open_chain_joints, - base_link, ee_link): +def get_kinematics_config( + robot, joint_id, link_id, open_chain_joints, base_link, ee_link +): joint_screws_in_ee = np.zeros((6, len(open_chain_joints))) ee_link_state = pb.getLinkState(robot, link_id[ee_link], 1, 1) if link_id[base_link] == -1: @@ -85,10 +82,12 @@ def get_kinematics_config(robot, joint_id, link_id, open_chain_joints, else: base_link_state = pb.getLinkState(robot, link_id[base_link], 1, 1) base_pos, base_quat = base_link_state[0], base_link_state[1] - T_w_b = liegroup.RpToTrans(util.quat_to_rot(np.array(base_quat)), - np.array(base_pos)) - T_w_ee = liegroup.RpToTrans(util.quat_to_rot(np.array(ee_link_state[1])), - np.array(ee_link_state[0])) + T_w_b = liegroup.RpToTrans( + util.quat_to_rot(np.array(base_quat)), np.array(base_pos) + ) + T_w_ee = liegroup.RpToTrans( + util.quat_to_rot(np.array(ee_link_state[1])), np.array(ee_link_state[0]) + ) T_b_ee = np.dot(liegroup.TransInv(T_w_b), T_w_ee) for i, joint_name in enumerate(open_chain_joints): joint_info = pb.getJointInfo(robot, joint_id[joint_name]) @@ -97,8 +96,9 @@ def get_kinematics_config(robot, joint_id, link_id, open_chain_joints, joint_axis = joint_info[13] screw_at_joint = np.zeros(6) link_state = pb.getLinkState(robot, link_id[link_name], 1, 1) - T_w_j = liegroup.RpToTrans(util.quat_to_rot(np.array(link_state[5])), - np.array(link_state[4])) + T_w_j = liegroup.RpToTrans( + util.quat_to_rot(np.array(link_state[5])), np.array(link_state[4]) + ) T_ee_j = np.dot(liegroup.TransInv(T_w_ee), T_w_j) Adj_ee_j = liegroup.Adjoint(T_ee_j) if joint_type == pb.JOINT_REVOLUTE: @@ -131,10 +131,9 @@ def get_link_vel(robot, link_idx): def set_link_damping(robot, link_id, lin_damping, ang_damping): for i in link_id.values(): - pb.changeDynamics(robot, - i, - linearDamping=lin_damping, - angularDamping=ang_damping) + pb.changeDynamics( + robot, i, linearDamping=lin_damping, angularDamping=ang_damping + ) def set_joint_friction(robot, joint_id, max_force=0): @@ -189,16 +188,17 @@ def draw_link_frame(robot, link_idx, linewidth=5.0, text=None): def set_motor_impedance(robot, joint_id, command, kp, kd): trq_applied = OrderedDict() for (joint_name, pos_des), (_, vel_des), (_, trq_des) in zip( - command["joint_positions_cmd_"].items(), - command["joint_velocities_cmd_"].items(), - command["joint_torques_cmd_"].items(), + command["joint_positions_cmd_"].items(), + command["joint_velocities_cmd_"].items(), + command["joint_torques_cmd_"].items(), ): joint_state = pb.getJointState(robot, joint_id[joint_name]) joint_pos, joint_vel = joint_state[0], joint_state[1] - trq_applied[joint_id[joint_name]] = (trq_des + kp[joint_name] * - (pos_des - joint_pos) + - kd[joint_name] * - (vel_des - joint_vel)) + trq_applied[joint_id[joint_name]] = ( + trq_des + + kp[joint_name] * (pos_des - joint_pos) + + kd[joint_name] * (vel_des - joint_vel) + ) pb.setJointMotorControlArray( robot, @@ -237,8 +237,7 @@ def set_motor_pos(robot, joint_id, pos_cmd): def set_motor_pos_vel(robot, joint_id, pos_cmd, vel_cmd): pos_applied = OrderedDict() vel_applied = OrderedDict() - for (joint_name, pos_des), (_, vel_des) in zip(pos_cmd.items(), - vel_cmd.items()): + for (joint_name, pos_des), (_, vel_des) in zip(pos_cmd.items(), vel_cmd.items()): pos_applied[joint_id[joint_name]] = pos_des vel_applied[joint_id[joint_name]] = vel_des @@ -251,8 +250,9 @@ def set_motor_pos_vel(robot, joint_id, pos_cmd, vel_cmd): ) -def get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, - rot_basejoint_to_basecom): +def get_sensor_data( + robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom +): """ Parameters ---------- @@ -306,13 +306,14 @@ def get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, sensor_data["base_com_ang_vel"] = np.asarray(base_com_ang_vel) rot_world_com = util.quat_to_rot(np.copy(sensor_data["base_com_quat"])) - rot_world_joint = np.dot(rot_world_com, - rot_basejoint_to_basecom.transpose()) + rot_world_joint = np.dot(rot_world_com, rot_basejoint_to_basecom.transpose()) sensor_data["base_joint_pos"] = sensor_data["base_com_pos"] - np.dot( - rot_world_joint, pos_basejoint_to_basecom) + rot_world_joint, pos_basejoint_to_basecom + ) sensor_data["base_joint_quat"] = util.rot_to_quat(rot_world_joint) - trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, - pos_basejoint_to_basecom) + trans_joint_com = liegroup.RpToTrans( + rot_basejoint_to_basecom, pos_basejoint_to_basecom + ) adT_joint_com = liegroup.Adjoint(trans_joint_com) twist_com_in_world = np.zeros(6) twist_com_in_world[0:3] = np.copy(sensor_data["base_com_ang_vel"]) @@ -322,8 +323,7 @@ def get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, augrot_com_world[3:6, 3:6] = rot_world_com.transpose() twist_com_in_com = np.dot(augrot_com_world, twist_com_in_world) twist_joint_in_joint = np.dot(adT_joint_com, twist_com_in_com) - rot_world_joint = np.dot(rot_world_com, - rot_basejoint_to_basecom.transpose()) + rot_world_joint = np.dot(rot_world_com, rot_basejoint_to_basecom.transpose()) augrot_world_joint = np.zeros((6, 6)) augrot_world_joint[0:3, 0:3] = rot_world_joint augrot_world_joint[3:6, 3:6] = rot_world_joint @@ -344,8 +344,7 @@ def get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, def simulate_dVel_data(robot, link_id, previous_link_velocity): # calculate imu acceleration in world frame by numerical differentiation - torso_dvel = (get_link_vel(robot, link_id['torso_imu'])[3:6] - - previous_link_velocity) + torso_dvel = get_link_vel(robot, link_id["torso_imu"])[3:6] - previous_link_velocity return torso_dvel @@ -364,11 +363,11 @@ def simulate_contact_sensor(force_sensor_measurement): return (force_sensor_measurement - voltage_bias) / voltage_to_force_map -def get_camera_image_from_link(robot, link, pic_width, pic_height, fov, - nearval, farval): +def get_camera_image_from_link( + robot, link, pic_width, pic_height, fov, nearval, farval +): aspect = pic_width / pic_height - projection_matrix = pb.computeProjectionMatrixFOV(fov, aspect, nearval, - farval) + projection_matrix = pb.computeProjectionMatrixFOV(fov, aspect, nearval, farval) link_info = pb.getLinkState(robot, link, 1, 1) # Get head link info link_pos = link_info[0] # Get link com pos wrt world link_ori = link_info[1] # Get link com ori wrt world @@ -381,8 +380,9 @@ def get_camera_image_from_link(robot, link, pic_width, pic_height, fov, camera_eye_pos = link_pos + np.dot(rot, 0.1 * global_camera_x_unit) camera_target_pos = link_pos + np.dot(rot, 1.0 * global_camera_x_unit) camera_up_vector = np.dot(rot, global_camera_z_unit) - view_matrix = pb.computeViewMatrix(camera_eye_pos, camera_target_pos, - camera_up_vector) # SE3_camera_to_world + view_matrix = pb.computeViewMatrix( + camera_eye_pos, camera_target_pos, camera_up_vector + ) # SE3_camera_to_world width, height, rgb_img, depth_img, seg_img = pb.getCameraImage( pic_width, # image width pic_height, # image height @@ -407,8 +407,7 @@ def add_sensor_noise(measurement, noise): def make_video(video_dir, delete_jpgs=True): images = [] - for file in tqdm(sorted(os.listdir(video_dir)), - desc="converting jpgs to gif"): + for file in tqdm(sorted(os.listdir(video_dir)), desc="converting jpgs to gif"): filename = video_dir + "/" + file im = cv2.imread(filename) im = im[:, :, [2, 1, 0]] # << BGR to RGB @@ -493,23 +492,20 @@ def set_config(robot, joint_id, base_pos, base_quat, joint_pos): pb.resetJointState(robot, joint_id[k], v, 0.0) -def get_point_cloud_data(depth_buffer, view_matrix, projection_matrix, d_hor, - d_ver): +def get_point_cloud_data(depth_buffer, view_matrix, projection_matrix, d_hor, d_ver): view_matrix = np.asarray(view_matrix).reshape([4, 4], order="F") - projection_matrix = np.asarray(projection_matrix).reshape([4, 4], - order="F") - trans_world_to_pix = np.linalg.inv( - np.matmul(projection_matrix, view_matrix)) + projection_matrix = np.asarray(projection_matrix).reshape([4, 4], order="F") + trans_world_to_pix = np.linalg.inv(np.matmul(projection_matrix, view_matrix)) # trans_camera_to_pix = np.linalg.inv(projection_matrix) img_height = (depth_buffer.shape)[0] img_width = (depth_buffer.shape)[1] wf_point_cloud_data = np.empty( - [np.int(img_height / d_ver), - np.int(img_width / d_hor), 3]) + [np.int(img_height / d_ver), np.int(img_width / d_hor), 3] + ) cf_point_cloud_data = np.empty( - [np.int(img_height / d_ver), - np.int(img_width / d_hor), 3]) + [np.int(img_height / d_ver), np.int(img_width / d_hor), 3] + ) for h in range(0, img_height, d_ver): for w in range(0, img_width, d_hor): @@ -519,14 +515,12 @@ def get_point_cloud_data(depth_buffer, view_matrix, projection_matrix, d_hor, pix_pos = np.asarray([x, y, z, 1]) point_in_world = np.matmul(trans_world_to_pix, pix_pos) # point_in_camera = np.matmul(trans_camera_to_pix, pix_pos) - wf_point_cloud_data[np.int(h / d_ver), - np.int(w / d_hor), :] = ( - point_in_world / - point_in_world[3])[:3] # world frame - - cf_point_cloud_data[np.int(h / d_ver), - np.int(w / d_hor), :] = ( - point_in_world / - point_in_world[3])[:3] # camera frame + wf_point_cloud_data[np.int(h / d_ver), np.int(w / d_hor), :] = ( + point_in_world / point_in_world[3] + )[:3] # world frame + + cf_point_cloud_data[np.int(h / d_ver), np.int(w / d_hor), :] = ( + point_in_world / point_in_world[3] + )[:3] # camera frame return wf_point_cloud_data, cf_point_cloud_data From 7879642379066e1a26232c9ab9116672a9400dca Mon Sep 17 00:00:00 2001 From: Carlos Gonzalez Date: Wed, 4 Sep 2024 16:16:53 -0500 Subject: [PATCH 4/5] aestehtics: reduced diameter of ICP visualization spheres --- UI/foxglove/UI_launcher.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/UI/foxglove/UI_launcher.py b/UI/foxglove/UI_launcher.py index 85ffb77c..5acacd84 100644 --- a/UI/foxglove/UI_launcher.py +++ b/UI/foxglove/UI_launcher.py @@ -238,8 +238,8 @@ async def main(): #add visual icp spheres icp_spheres = ShapeScene() - icp_spheres.add_shape("est_icp", "spheres", [1, 0, 1, 1], [0.1, 0.1, 0.1]) - icp_spheres.add_shape("des_icp", "spheres", [0, 1, 0, 1], [0.1, 0.1, 0.1]) + icp_spheres.add_shape("est_icp", "spheres", [1, 0, 1, 1], [0.03, 0.03, 0.03]) + icp_spheres.add_shape("des_icp", "spheres", [0, 1, 0, 1], [0.03, 0.03, 0.03]) # Send the FrameTransform every frame to update the model's position transform = FrameTransform() @@ -263,7 +263,7 @@ async def main(): msg.ParseFromString(encoded_msg) check_if_kf_estimator(msg.kf_base_joint_pos, msg.est_base_joint_pos) - await asyncio.sleep(0.02) + await asyncio.sleep(0.01) now = time.time_ns() transform.timestamp.FromNanoseconds(now) From f997b34c18427c33f95d620073210304c04b4822 Mon Sep 17 00:00:00 2001 From: Carlos Gonzalez Date: Wed, 4 Sep 2024 17:40:13 -0500 Subject: [PATCH 5/5] fixed formatting errors for linters --- .pre-commit-config.yaml | 2 +- UI/foxglove/UI_launcher.py | 15 +++++++-------- mujoco/include/mujoco/mjdata.h | 2 +- scripts/real_teleop.py | 9 +++++---- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 72525ddb..e30d7a1a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -19,7 +19,7 @@ repos: hooks: - id: ruff # ignore import-star and import-star-usage - args: [--fix, --ignore, F403, --ignore, F405] + args: [--fix, --ignore, F403, --ignore, F405, --ignore, E402] - id: ruff-format - repo: https://github.com/pre-commit/pre-commit-hooks rev: v4.6.0 diff --git a/UI/foxglove/UI_launcher.py b/UI/foxglove/UI_launcher.py index 8b5fe469..4eb68ca0 100644 --- a/UI/foxglove/UI_launcher.py +++ b/UI/foxglove/UI_launcher.py @@ -173,13 +173,13 @@ async def main(): icp_des_chan_id = await SceneChannel( True, "icp_des", "json", "icp_des", ["x", "y"] ).add_chan(server) - proj_footstepS_chan_id = await SceneChannel( - False, - "proj_footstep_viz", - "protobuf", - SceneUpdate.DESCRIPTOR.full_name, - scene_schema, - ).add_chan(server) + # proj_footstepS_chan_id = await SceneChannel( + # False, + # "proj_footstep_viz", + # "protobuf", + # SceneUpdate.DESCRIPTOR.full_name, + # scene_schema, + # ).add_chan(server) b_ft_contact_chan_id = await SceneChannel( True, "b_ft_contact", "json", "b_ft_contact", ["b_lfoot", "b_rfoot"] ).add_chan(server) @@ -279,7 +279,6 @@ async def main(): transform = FrameTransform() print("foxglove websocket initiated") - r_foot, l_foot = [0, 0, 0], [0, 0, 0] # Clear experiment_data to start new footstep planning fp.remove_yaml_files(fp.WATCHED_DIR) diff --git a/mujoco/include/mujoco/mjdata.h b/mujoco/include/mujoco/mjdata.h index 9b4a26c0..f5619ad1 100644 --- a/mujoco/include/mujoco/mjdata.h +++ b/mujoco/include/mujoco/mjdata.h @@ -479,7 +479,7 @@ struct mjData_ { *efc_aref; // reference pseudo-acceleration (nefc x 1) //-------------------- arena-allocated: POSITION, VELOCITY, - //CONTROL/ACCELERATION dependent + // CONTROL/ACCELERATION dependent // computed by mj_fwdConstraint/mj_inverse mjtNum *efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) diff --git a/scripts/real_teleop.py b/scripts/real_teleop.py index f35075e0..ed4a181d 100644 --- a/scripts/real_teleop.py +++ b/scripts/real_teleop.py @@ -10,6 +10,7 @@ from util.python_utils.device.t265 import T265 import util.python_utils.util as util +import matplotlib.pyplot as plt import time import cv2 @@ -71,7 +72,7 @@ def _on_press(self, key): if key_char == "e": self._t_last_click = -1 self._t_click = time.time() - elapsed_time = self._t_click - self._t_last_click + # elapsed_time = self._t_click - self._t_last_click self._t_last_click = self._t_click self.single_click_and_hold = True elif key_char == "s": @@ -298,12 +299,12 @@ def visualize(data_path): data["right_img"] = [] left_img = np.zeros((800, 848, 3), dtype="uint8") right_img = np.zeros((800, 848, 3), dtype="uint8") - for time in data["time"]: + for tm in data["time"]: left_img_read = cv2.imread( - os.path.join(left_img_dir, "{}.png".format(int(time * 100))) + os.path.join(left_img_dir, "{}.png".format(int(tm * 100))) ) right_img_read = cv2.imread( - os.path.join(right_img_dir, "{}.png".format(int(time * 100))) + os.path.join(right_img_dir, "{}.png".format(int(tm * 100))) ) data["left_img"] += [left_img] data["right_img"] += [right_img]