Commit 0a1ce876 authored by Nicolas Van der Noot's avatar Nicolas Van der Noot

remove user_kinematics_model

parent 4db217ff
......@@ -294,12 +294,8 @@ The following procedure can be used to generate new models of the COMAN robot (o
* Launch `./Gen_mds_user path_to_mbs user_all_id`
* Move the created file in the new folder: `mv userfctR/user_all_id.h symbolicR/new_version/`
* Go in the [dataR/extra](dataR/extra) folder, and create the folder *new_version*. Create the related *specific_simu_index.cc* file.
* Generate the *user_kinematics_model.c* file like this:
* Generate the *accelred* file (*mbs_accelred_new_version.c*) where the F sensors are set for *Position*, *Rotation Matrix* and *Velocity* and place this file in [this folder](userFiles/simu/forces/contactGeom/python).
* Go inside [the same folder](userFiles/simu/forces/contactGeom/python) and launch `python kinematics_gen.py mbs_accelred_new_version.c ../../../../../symbolicR/new_version/user_kinematics_model.c`
* Re-generate the *accelred* file, but unckecking *Position*, *Rotation Matrix* and *Velocity* (for computational speed) and place it in *symbolicR/new_version*.
* In [CMakeLists.txt](workR/CMakeLists.txt) change `set( WALKER_VERSION "old_version" )` by `set( WALKER_VERSION "new_version" )`
* Add a new COMAN version name *NEW_NAME* in [SimuOptions.h](userFiles/simu/io/simu_options/SimuOptions.h).
* Add a new version name *NEW_NAME* in [SimuOptions.h](userFiles/simu/io/simu_options/SimuOptions.h).
* Add the line `else if (!strcmp(WALKER_VERSION, "new_version")) return NEW_NAME;` in [SimuOptions.c](userFiles/simu/io/simu_options/SimuOptions.c).
* Adapt the files in the [indexes](userFiles/simu/main/indexes) folder ([SimuIndex.cc](userFiles/simu/main/indexes/SimuIndex.cc), [SimuIndex.hh](userFiles/simu/main/indexes/SimuIndex.hh), [ModelSimuIndex.cc](userFiles/simu/main/indexes/ModelSimuIndex.cc) and [ModelSimuIndex.hh](userFiles/simu/main/indexes/ModelSimuIndex.hh)).
* You might also have to adapt the following files:
......
This source diff could not be displayed because it is too large. You can view the blob instead.
#include "mbs_data.h"
#include "math.h"
/*! \brief compute the kinematics model of sensors with ExtForces
*
* \param[in] mbs_data Robotran structure
* \param[out] PxF_tab tabular with all the 'PxF' computed
* \param[out] RxF_tab tabular with all the 'RxF' computed
* \param[out] VxF_tab tabular with all the 'VxF' computed
* \param[out] OMxF_tab tabular with all the 'OMxF' computed
*/
void user_kinematics_model(MbsData *mbs_data, double **PxF_tab, double ***RxF_tab, double **VxF_tab, double **OMxF_tab)
{
// variables declaration
int i, j;
double *q, *qd;
double **dpt;
double PxF1[4], PxF2[4], PxF3[4], PxF4[4], PxF5[4], PxF6[4], PxF7[4], PxF8[4], PxF9[4], PxF10[4], PxF11[4], PxF12[4];
double RxF1[4][4], RxF2[4][4], RxF3[4][4], RxF4[4][4], RxF5[4][4], RxF6[4][4], RxF7[4][4], RxF8[4][4], RxF9[4][4], RxF10[4][4], RxF11[4][4], RxF12[4][4];
double VxF1[4], VxF2[4], VxF3[4], VxF4[4], VxF5[4], VxF6[4], VxF7[4], VxF8[4], VxF9[4], VxF10[4], VxF11[4], VxF12[4];
double OMxF1[4], OMxF2[4], OMxF3[4], OMxF4[4], OMxF5[4], OMxF6[4], OMxF7[4], OMxF8[4], OMxF9[4], OMxF10[4], OMxF11[4], OMxF12[4];
double C4, C5, C6, C7, C8, C9, C10, C11, C12, C13, C14, C15, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26, C27, C28, C29;
double S4, S5, S6, S7, S8, S9, S10, S11, S12, S13, S14, S15, S16, S17, S18, S19, S20, S21, S22, S23, S24, S25, S26, S27, S28, S29;
double ROcp25_25, ROcp25_35, ROcp25_85, ROcp25_95, ROcp25_16, ROcp25_26, ROcp25_36, ROcp25_46, ROcp25_56, ROcp25_66, OMcp25_25, OMcp25_35, OMcp25_16, OMcp25_26, OMcp25_36;
double ROcp26_25, ROcp26_35, ROcp26_85, ROcp26_95, ROcp26_16, ROcp26_26, ROcp26_36, ROcp26_46, ROcp26_56, ROcp26_66, ROcp26_17, ROcp26_27, ROcp26_37, ROcp26_77, ROcp26_87;
double ROcp26_97, ROcp26_48, ROcp26_58, ROcp26_68, ROcp26_78, ROcp26_88, ROcp26_98, ROcp26_19, ROcp26_29, ROcp26_39, ROcp26_49, ROcp26_59, ROcp26_69, OMcp26_25, OMcp26_35;
double OMcp26_16, OMcp26_26, OMcp26_36, RLcp26_17, RLcp26_27, RLcp26_37, POcp26_17, POcp26_27, POcp26_37, OMcp26_17, OMcp26_27, OMcp26_37, ORcp26_17, ORcp26_27, ORcp26_37;
double VIcp26_17, VIcp26_27, VIcp26_37, RLcp26_18, RLcp26_28, RLcp26_38, POcp26_18, POcp26_28, POcp26_38, OMcp26_18, OMcp26_28, OMcp26_38, ORcp26_18, ORcp26_28, ORcp26_38;
double VIcp26_18, VIcp26_28, VIcp26_38, RLcp26_19, RLcp26_29, RLcp26_39, POcp26_19, POcp26_29, POcp26_39, OMcp26_19, OMcp26_29, OMcp26_39, ORcp26_19, ORcp26_29, ORcp26_39;
double VIcp26_19, VIcp26_29, VIcp26_39, ROcp27_25, ROcp27_35, ROcp27_85, ROcp27_95, ROcp27_16, ROcp27_26, ROcp27_36, ROcp27_46, ROcp27_56, ROcp27_66, ROcp27_17, ROcp27_27;
double ROcp27_37, ROcp27_77, ROcp27_87, ROcp27_97, ROcp27_48, ROcp27_58, ROcp27_68, ROcp27_78, ROcp27_88, ROcp27_98, ROcp27_19, ROcp27_29, ROcp27_39, ROcp27_49, ROcp27_59;
double ROcp27_69, ROcp27_110, ROcp27_210, ROcp27_310, ROcp27_710, ROcp27_810, ROcp27_910, OMcp27_25, OMcp27_35, OMcp27_16, OMcp27_26, OMcp27_36, RLcp27_17, RLcp27_27, RLcp27_37;
double POcp27_17, POcp27_27, POcp27_37, OMcp27_17, OMcp27_27, OMcp27_37, ORcp27_17, ORcp27_27, ORcp27_37, VIcp27_17, VIcp27_27, VIcp27_37, RLcp27_18, RLcp27_28, RLcp27_38;
double POcp27_18, POcp27_28, POcp27_38, OMcp27_18, OMcp27_28, OMcp27_38, ORcp27_18, ORcp27_28, ORcp27_38, VIcp27_18, VIcp27_28, VIcp27_38, RLcp27_19, RLcp27_29, RLcp27_39;
double POcp27_19, POcp27_29, POcp27_39, OMcp27_19, OMcp27_29, OMcp27_39, ORcp27_19, ORcp27_29, ORcp27_39, VIcp27_19, VIcp27_29, VIcp27_39, RLcp27_110, RLcp27_210, RLcp27_310;
double POcp27_110, POcp27_210, POcp27_310, OMcp27_110, OMcp27_210, OMcp27_310, ORcp27_110, ORcp27_210, ORcp27_310, VIcp27_110, VIcp27_210, VIcp27_310, ROcp28_25, ROcp28_35, ROcp28_85;
double ROcp28_95, ROcp28_16, ROcp28_26, ROcp28_36, ROcp28_46, ROcp28_56, ROcp28_66, ROcp28_17, ROcp28_27, ROcp28_37, ROcp28_77, ROcp28_87, ROcp28_97, ROcp28_48, ROcp28_58;
double ROcp28_68, ROcp28_78, ROcp28_88, ROcp28_98, ROcp28_19, ROcp28_29, ROcp28_39, ROcp28_49, ROcp28_59, ROcp28_69, ROcp28_110, ROcp28_210, ROcp28_310, ROcp28_710, ROcp28_810;
double ROcp28_910, ROcp28_411, ROcp28_511, ROcp28_611, ROcp28_711, ROcp28_811, ROcp28_911, ROcp28_112, ROcp28_212, ROcp28_312, ROcp28_712, ROcp28_812, ROcp28_912, OMcp28_25, OMcp28_35;
double OMcp28_16, OMcp28_26, OMcp28_36, RLcp28_17, RLcp28_27, RLcp28_37, POcp28_17, POcp28_27, POcp28_37, OMcp28_17, OMcp28_27, OMcp28_37, ORcp28_17, ORcp28_27, ORcp28_37;
double VIcp28_17, VIcp28_27, VIcp28_37, RLcp28_18, RLcp28_28, RLcp28_38, POcp28_18, POcp28_28, POcp28_38, OMcp28_18, OMcp28_28, OMcp28_38, ORcp28_18, ORcp28_28, ORcp28_38;
double VIcp28_18, VIcp28_28, VIcp28_38, RLcp28_19, RLcp28_29, RLcp28_39, POcp28_19, POcp28_29, POcp28_39, OMcp28_19, OMcp28_29, OMcp28_39, ORcp28_19, ORcp28_29, ORcp28_39;
double VIcp28_19, VIcp28_29, VIcp28_39, RLcp28_110, RLcp28_210, RLcp28_310, POcp28_110, POcp28_210, POcp28_310, OMcp28_110, OMcp28_210, OMcp28_310, ORcp28_110, ORcp28_210, ORcp28_310;
double VIcp28_110, VIcp28_210, VIcp28_310, RLcp28_111, RLcp28_211, RLcp28_311, POcp28_111, POcp28_211, POcp28_311, OMcp28_111, OMcp28_211, OMcp28_311, ORcp28_111, ORcp28_211, ORcp28_311;
double VIcp28_111, VIcp28_211, VIcp28_311, OMcp28_112, OMcp28_212, OMcp28_312, RLcp28_113, RLcp28_213, RLcp28_313, POcp28_113, POcp28_213, POcp28_313, ORcp28_113, ORcp28_213, ORcp28_313;
double VIcp28_113, VIcp28_213, VIcp28_313, ROcp29_25, ROcp29_35, ROcp29_85, ROcp29_95, ROcp29_16, ROcp29_26, ROcp29_36, ROcp29_46, ROcp29_56, ROcp29_66, ROcp29_113, ROcp29_213;
double ROcp29_313, ROcp29_713, ROcp29_813, ROcp29_913, ROcp29_414, ROcp29_514, ROcp29_614, ROcp29_714, ROcp29_814, ROcp29_914, ROcp29_115, ROcp29_215, ROcp29_315, ROcp29_415, ROcp29_515;
double ROcp29_615, OMcp29_25, OMcp29_35, OMcp29_16, OMcp29_26, OMcp29_36, RLcp29_17, RLcp29_27, RLcp29_37, POcp29_17, POcp29_27, POcp29_37, OMcp29_17, OMcp29_27, OMcp29_37;
double ORcp29_17, ORcp29_27, ORcp29_37, VIcp29_17, VIcp29_27, VIcp29_37, RLcp29_18, RLcp29_28, RLcp29_38, POcp29_18, POcp29_28, POcp29_38, OMcp29_18, OMcp29_28, OMcp29_38;
double ORcp29_18, ORcp29_28, ORcp29_38, VIcp29_18, VIcp29_28, VIcp29_38, RLcp29_19, RLcp29_29, RLcp29_39, POcp29_19, POcp29_29, POcp29_39, OMcp29_19, OMcp29_29, OMcp29_39;
double ORcp29_19, ORcp29_29, ORcp29_39, VIcp29_19, VIcp29_29, VIcp29_39, ROcp30_25, ROcp30_35, ROcp30_85, ROcp30_95, ROcp30_16, ROcp30_26, ROcp30_36, ROcp30_46, ROcp30_56;
double ROcp30_66, ROcp30_113, ROcp30_213, ROcp30_313, ROcp30_713, ROcp30_813, ROcp30_913, ROcp30_414, ROcp30_514, ROcp30_614, ROcp30_714, ROcp30_814, ROcp30_914, ROcp30_115, ROcp30_215;
double ROcp30_315, ROcp30_415, ROcp30_515, ROcp30_615, ROcp30_116, ROcp30_216, ROcp30_316, ROcp30_716, ROcp30_816, ROcp30_916, OMcp30_25, OMcp30_35, OMcp30_16, OMcp30_26, OMcp30_36;
double RLcp30_17, RLcp30_27, RLcp30_37, POcp30_17, POcp30_27, POcp30_37, OMcp30_17, OMcp30_27, OMcp30_37, ORcp30_17, ORcp30_27, ORcp30_37, VIcp30_17, VIcp30_27, VIcp30_37;
double RLcp30_18, RLcp30_28, RLcp30_38, POcp30_18, POcp30_28, POcp30_38, OMcp30_18, OMcp30_28, OMcp30_38, ORcp30_18, ORcp30_28, ORcp30_38, VIcp30_18, VIcp30_28, VIcp30_38;
double RLcp30_19, RLcp30_29, RLcp30_39, POcp30_19, POcp30_29, POcp30_39, OMcp30_19, OMcp30_29, OMcp30_39, ORcp30_19, ORcp30_29, ORcp30_39, VIcp30_19, VIcp30_29, VIcp30_39;
double RLcp30_110, RLcp30_210, RLcp30_310, POcp30_110, POcp30_210, POcp30_310, OMcp30_110, OMcp30_210, OMcp30_310, ORcp30_110, ORcp30_210, ORcp30_310, VIcp30_110, VIcp30_210, VIcp30_310;
double ROcp31_25, ROcp31_35, ROcp31_85, ROcp31_95, ROcp31_16, ROcp31_26, ROcp31_36, ROcp31_46, ROcp31_56, ROcp31_66, ROcp31_113, ROcp31_213, ROcp31_313, ROcp31_713, ROcp31_813;
double ROcp31_913, ROcp31_414, ROcp31_514, ROcp31_614, ROcp31_714, ROcp31_814, ROcp31_914, ROcp31_115, ROcp31_215, ROcp31_315, ROcp31_415, ROcp31_515, ROcp31_615, ROcp31_116, ROcp31_216;
double ROcp31_316, ROcp31_716, ROcp31_816, ROcp31_916, ROcp31_417, ROcp31_517, ROcp31_617, ROcp31_717, ROcp31_817, ROcp31_917, ROcp31_118, ROcp31_218, ROcp31_318, ROcp31_718, ROcp31_818;
double ROcp31_918, OMcp31_25, OMcp31_35, OMcp31_16, OMcp31_26, OMcp31_36, RLcp31_17, RLcp31_27, RLcp31_37, POcp31_17, POcp31_27, POcp31_37, OMcp31_17, OMcp31_27, OMcp31_37;
double ORcp31_17, ORcp31_27, ORcp31_37, VIcp31_17, VIcp31_27, VIcp31_37, RLcp31_18, RLcp31_28, RLcp31_38, POcp31_18, POcp31_28, POcp31_38, OMcp31_18, OMcp31_28, OMcp31_38;
double ORcp31_18, ORcp31_28, ORcp31_38, VIcp31_18, VIcp31_28, VIcp31_38, RLcp31_19, RLcp31_29, RLcp31_39, POcp31_19, POcp31_29, POcp31_39, OMcp31_19, OMcp31_29, OMcp31_39;
double ORcp31_19, ORcp31_29, ORcp31_39, VIcp31_19, VIcp31_29, VIcp31_39, RLcp31_110, RLcp31_210, RLcp31_310, POcp31_110, POcp31_210, POcp31_310, OMcp31_110, OMcp31_210, OMcp31_310;
double ORcp31_110, ORcp31_210, ORcp31_310, VIcp31_110, VIcp31_210, VIcp31_310, RLcp31_111, RLcp31_211, RLcp31_311, POcp31_111, POcp31_211, POcp31_311, OMcp31_111, OMcp31_211, OMcp31_311;
double ORcp31_111, ORcp31_211, ORcp31_311, VIcp31_111, VIcp31_211, VIcp31_311, OMcp31_112, OMcp31_212, OMcp31_312, RLcp31_113, RLcp31_213, RLcp31_313, POcp31_113, POcp31_213, POcp31_313;
double ORcp31_113, ORcp31_213, ORcp31_313, VIcp31_113, VIcp31_213, VIcp31_313, ROcp32_25, ROcp32_35, ROcp32_85, ROcp32_95, ROcp32_16, ROcp32_26, ROcp32_36, ROcp32_46, ROcp32_56;
double ROcp32_66, ROcp32_419, ROcp32_519, ROcp32_619, ROcp32_719, ROcp32_819, ROcp32_919, ROcp32_120, ROcp32_220, ROcp32_320, ROcp32_720, ROcp32_820, ROcp32_920, ROcp32_121, ROcp32_221;
double ROcp32_321, ROcp32_421, ROcp32_521, ROcp32_621, OMcp32_25, OMcp32_35, OMcp32_16, OMcp32_26, OMcp32_36, RLcp32_17, RLcp32_27, RLcp32_37, POcp32_17, POcp32_27, POcp32_37;
double OMcp32_17, OMcp32_27, OMcp32_37, ORcp32_17, ORcp32_27, ORcp32_37, VIcp32_17, VIcp32_27, VIcp32_37, OMcp32_18, OMcp32_28, OMcp32_38, RLcp32_19, RLcp32_29, RLcp32_39;
double POcp32_19, POcp32_29, POcp32_39, OMcp32_19, OMcp32_29, OMcp32_39, ORcp32_19, ORcp32_29, ORcp32_39, VIcp32_19, VIcp32_29, VIcp32_39, ROcp33_25, ROcp33_35, ROcp33_85;
double ROcp33_95, ROcp33_16, ROcp33_26, ROcp33_36, ROcp33_46, ROcp33_56, ROcp33_66, ROcp33_419, ROcp33_519, ROcp33_619, ROcp33_719, ROcp33_819, ROcp33_919, ROcp33_120, ROcp33_220;
double ROcp33_320, ROcp33_720, ROcp33_820, ROcp33_920, ROcp33_121, ROcp33_221, ROcp33_321, ROcp33_421, ROcp33_521, ROcp33_621, ROcp33_122, ROcp33_222, ROcp33_322, ROcp33_722, ROcp33_822;
double ROcp33_922, ROcp33_423, ROcp33_523, ROcp33_623, ROcp33_723, ROcp33_823, ROcp33_923, ROcp33_124, ROcp33_224, ROcp33_324, ROcp33_424, ROcp33_524, ROcp33_624, OMcp33_25, OMcp33_35;
double OMcp33_16, OMcp33_26, OMcp33_36, RLcp33_17, RLcp33_27, RLcp33_37, POcp33_17, POcp33_27, POcp33_37, OMcp33_17, OMcp33_27, OMcp33_37, ORcp33_17, ORcp33_27, ORcp33_37;
double VIcp33_17, VIcp33_27, VIcp33_37, OMcp33_18, OMcp33_28, OMcp33_38, RLcp33_19, RLcp33_29, RLcp33_39, POcp33_19, POcp33_29, POcp33_39, OMcp33_19, OMcp33_29, OMcp33_39;
double ORcp33_19, ORcp33_29, ORcp33_39, VIcp33_19, VIcp33_29, VIcp33_39, RLcp33_110, RLcp33_210, RLcp33_310, POcp33_110, POcp33_210, POcp33_310, OMcp33_110, OMcp33_210, OMcp33_310;
double ORcp33_110, ORcp33_210, ORcp33_310, VIcp33_110, VIcp33_210, VIcp33_310, RLcp33_111, RLcp33_211, RLcp33_311, POcp33_111, POcp33_211, POcp33_311, OMcp33_111, OMcp33_211, OMcp33_311;
double ORcp33_111, ORcp33_211, ORcp33_311, VIcp33_111, VIcp33_211, VIcp33_311, RLcp33_112, RLcp33_212, RLcp33_312, POcp33_112, POcp33_212, POcp33_312, OMcp33_112, OMcp33_212, OMcp33_312;
double ORcp33_112, ORcp33_212, ORcp33_312, VIcp33_112, VIcp33_212, VIcp33_312, ROcp34_25, ROcp34_35, ROcp34_85, ROcp34_95, ROcp34_16, ROcp34_26, ROcp34_36, ROcp34_46, ROcp34_56;
double ROcp34_66, ROcp34_419, ROcp34_519, ROcp34_619, ROcp34_719, ROcp34_819, ROcp34_919, ROcp34_120, ROcp34_220, ROcp34_320, ROcp34_720, ROcp34_820, ROcp34_920, ROcp34_121, ROcp34_221;
double ROcp34_321, ROcp34_421, ROcp34_521, ROcp34_621, ROcp34_122, ROcp34_222, ROcp34_322, ROcp34_722, ROcp34_822, ROcp34_922, ROcp34_423, ROcp34_523, ROcp34_623, ROcp34_723, ROcp34_823;
double ROcp34_923, ROcp34_124, ROcp34_224, ROcp34_324, ROcp34_424, ROcp34_524, ROcp34_624, ROcp34_125, ROcp34_225, ROcp34_325, ROcp34_725, ROcp34_825, ROcp34_925, OMcp34_25, OMcp34_35;
double OMcp34_16, OMcp34_26, OMcp34_36, RLcp34_17, RLcp34_27, RLcp34_37, POcp34_17, POcp34_27, POcp34_37, OMcp34_17, OMcp34_27, OMcp34_37, ORcp34_17, ORcp34_27, ORcp34_37;
double VIcp34_17, VIcp34_27, VIcp34_37, OMcp34_18, OMcp34_28, OMcp34_38, RLcp34_19, RLcp34_29, RLcp34_39, POcp34_19, POcp34_29, POcp34_39, OMcp34_19, OMcp34_29, OMcp34_39;
double ORcp34_19, ORcp34_29, ORcp34_39, VIcp34_19, VIcp34_29, VIcp34_39, RLcp34_110, RLcp34_210, RLcp34_310, POcp34_110, POcp34_210, POcp34_310, OMcp34_110, OMcp34_210, OMcp34_310;
double ORcp34_110, ORcp34_210, ORcp34_310, VIcp34_110, VIcp34_210, VIcp34_310, RLcp34_111, RLcp34_211, RLcp34_311, POcp34_111, POcp34_211, POcp34_311, OMcp34_111, OMcp34_211, OMcp34_311;
double ORcp34_111, ORcp34_211, ORcp34_311, VIcp34_111, VIcp34_211, VIcp34_311, RLcp34_112, RLcp34_212, RLcp34_312, POcp34_112, POcp34_212, POcp34_312, OMcp34_112, OMcp34_212, OMcp34_312;
double ORcp34_112, ORcp34_212, ORcp34_312, VIcp34_112, VIcp34_212, VIcp34_312, RLcp34_113, RLcp34_213, RLcp34_313, POcp34_113, POcp34_213, POcp34_313, OMcp34_113, OMcp34_213, OMcp34_313;
double ORcp34_113, ORcp34_213, ORcp34_313, VIcp34_113, VIcp34_213, VIcp34_313, ROcp35_25, ROcp35_35, ROcp35_85, ROcp35_95, ROcp35_16, ROcp35_26, ROcp35_36, ROcp35_46, ROcp35_56;
double ROcp35_66, ROcp35_419, ROcp35_519, ROcp35_619, ROcp35_719, ROcp35_819, ROcp35_919, ROcp35_120, ROcp35_220, ROcp35_320, ROcp35_720, ROcp35_820, ROcp35_920, ROcp35_121, ROcp35_221;
double ROcp35_321, ROcp35_421, ROcp35_521, ROcp35_621, ROcp35_126, ROcp35_226, ROcp35_326, ROcp35_726, ROcp35_826, ROcp35_926, ROcp35_427, ROcp35_527, ROcp35_627, ROcp35_727, ROcp35_827;
double ROcp35_927, ROcp35_128, ROcp35_228, ROcp35_328, ROcp35_428, ROcp35_528, ROcp35_628, OMcp35_25, OMcp35_35, OMcp35_16, OMcp35_26, OMcp35_36, RLcp35_17, RLcp35_27, RLcp35_37;
double POcp35_17, POcp35_27, POcp35_37, OMcp35_17, OMcp35_27, OMcp35_37, ORcp35_17, ORcp35_27, ORcp35_37, VIcp35_17, VIcp35_27, VIcp35_37, OMcp35_18, OMcp35_28, OMcp35_38;
double RLcp35_19, RLcp35_29, RLcp35_39, POcp35_19, POcp35_29, POcp35_39, OMcp35_19, OMcp35_29, OMcp35_39, ORcp35_19, ORcp35_29, ORcp35_39, VIcp35_19, VIcp35_29, VIcp35_39;
double RLcp35_110, RLcp35_210, RLcp35_310, POcp35_110, POcp35_210, POcp35_310, OMcp35_110, OMcp35_210, OMcp35_310, ORcp35_110, ORcp35_210, ORcp35_310, VIcp35_110, VIcp35_210, VIcp35_310;
double RLcp35_111, RLcp35_211, RLcp35_311, POcp35_111, POcp35_211, POcp35_311, OMcp35_111, OMcp35_211, OMcp35_311, ORcp35_111, ORcp35_211, ORcp35_311, VIcp35_111, VIcp35_211, VIcp35_311;
double RLcp35_112, RLcp35_212, RLcp35_312, POcp35_112, POcp35_212, POcp35_312, OMcp35_112, OMcp35_212, OMcp35_312, ORcp35_112, ORcp35_212, ORcp35_312, VIcp35_112, VIcp35_212, VIcp35_312;
double ROcp36_25, ROcp36_35, ROcp36_85, ROcp36_95, ROcp36_16, ROcp36_26, ROcp36_36, ROcp36_46, ROcp36_56, ROcp36_66, ROcp36_419, ROcp36_519, ROcp36_619, ROcp36_719, ROcp36_819;
double ROcp36_919, ROcp36_120, ROcp36_220, ROcp36_320, ROcp36_720, ROcp36_820, ROcp36_920, ROcp36_121, ROcp36_221, ROcp36_321, ROcp36_421, ROcp36_521, ROcp36_621, ROcp36_126, ROcp36_226;
double ROcp36_326, ROcp36_726, ROcp36_826, ROcp36_926, ROcp36_427, ROcp36_527, ROcp36_627, ROcp36_727, ROcp36_827, ROcp36_927, ROcp36_128, ROcp36_228, ROcp36_328, ROcp36_428, ROcp36_528;
double ROcp36_628, ROcp36_129, ROcp36_229, ROcp36_329, ROcp36_729, ROcp36_829, ROcp36_929, OMcp36_25, OMcp36_35, OMcp36_16, OMcp36_26, OMcp36_36, RLcp36_17, RLcp36_27, RLcp36_37;
double POcp36_17, POcp36_27, POcp36_37, OMcp36_17, OMcp36_27, OMcp36_37, ORcp36_17, ORcp36_27, ORcp36_37, VIcp36_17, VIcp36_27, VIcp36_37, OMcp36_18, OMcp36_28, OMcp36_38;
double RLcp36_19, RLcp36_29, RLcp36_39, POcp36_19, POcp36_29, POcp36_39, OMcp36_19, OMcp36_29, OMcp36_39, ORcp36_19, ORcp36_29, ORcp36_39, VIcp36_19, VIcp36_29, VIcp36_39;
double RLcp36_110, RLcp36_210, RLcp36_310, POcp36_110, POcp36_210, POcp36_310, OMcp36_110, OMcp36_210, OMcp36_310, ORcp36_110, ORcp36_210, ORcp36_310, VIcp36_110, VIcp36_210, VIcp36_310;
double RLcp36_111, RLcp36_211, RLcp36_311, POcp36_111, POcp36_211, POcp36_311, OMcp36_111, OMcp36_211, OMcp36_311, ORcp36_111, ORcp36_211, ORcp36_311, VIcp36_111, VIcp36_211, VIcp36_311;
double RLcp36_112, RLcp36_212, RLcp36_312, POcp36_112, POcp36_212, POcp36_312, OMcp36_112, OMcp36_212, OMcp36_312, ORcp36_112, ORcp36_212, ORcp36_312, VIcp36_112, VIcp36_212, VIcp36_312;
double RLcp36_113, RLcp36_213, RLcp36_313, POcp36_113, POcp36_213, POcp36_313, OMcp36_113, OMcp36_213, OMcp36_313, ORcp36_113, ORcp36_213, ORcp36_313, VIcp36_113, VIcp36_213, VIcp36_313;
// variables initialization
q = mbs_data->q;
qd = mbs_data->qd;
dpt = mbs_data->dpt;
// cosines and sines
C4 = cos(q[4]);
C5 = cos(q[5]);
C6 = cos(q[6]);
C7 = cos(q[7]);
C8 = cos(q[8]);
C9 = cos(q[9]);
C10 = cos(q[10]);
C11 = cos(q[11]);
C12 = cos(q[12]);
C13 = cos(q[13]);
C14 = cos(q[14]);
C15 = cos(q[15]);
C16 = cos(q[16]);
C17 = cos(q[17]);
C18 = cos(q[18]);
C19 = cos(q[19]);
C20 = cos(q[20]);
C21 = cos(q[21]);
C22 = cos(q[22]);
C23 = cos(q[23]);
C24 = cos(q[24]);
C25 = cos(q[25]);
C26 = cos(q[26]);
C27 = cos(q[27]);
C28 = cos(q[28]);
C29 = cos(q[29]);
S4 = sin(q[4]);
S5 = sin(q[5]);
S6 = sin(q[6]);
S7 = sin(q[7]);
S8 = sin(q[8]);
S9 = sin(q[9]);
S10 = sin(q[10]);
S11 = sin(q[11]);
S12 = sin(q[12]);
S13 = sin(q[13]);
S14 = sin(q[14]);
S15 = sin(q[15]);
S16 = sin(q[16]);
S17 = sin(q[17]);
S18 = sin(q[18]);
S19 = sin(q[19]);
S20 = sin(q[20]);
S21 = sin(q[21]);
S22 = sin(q[22]);
S23 = sin(q[23]);
S24 = sin(q[24]);
S25 = sin(q[25]);
S26 = sin(q[26]);
S27 = sin(q[27]);
S28 = sin(q[28]);
S29 = sin(q[29]);
// main computation
ROcp25_25 = S4*S5;
ROcp25_35 = -C4*S5;
ROcp25_85 = -S4*C5;
ROcp25_95 = C4*C5;
ROcp25_16 = C5*C6;
ROcp25_26 = ROcp25_25*C6+C4*S6;
ROcp25_36 = ROcp25_35*C6+S4*S6;
ROcp25_46 = -C5*S6;
ROcp25_56 = -ROcp25_25*S6+C4*C6;
ROcp25_66 = -ROcp25_35*S6+S4*C6;
OMcp25_25 = qd[5]*C4;
OMcp25_35 = qd[5]*S4;
OMcp25_16 = qd[4]+qd[6]*S5;
OMcp25_26 = OMcp25_25+qd[6]*ROcp25_85;
OMcp25_36 = OMcp25_35+qd[6]*ROcp25_95;
PxF1[1] = q[1];
PxF1[2] = q[2];
PxF1[3] = q[3];
RxF1[1][1] = ROcp25_16;
RxF1[1][2] = ROcp25_26;
RxF1[1][3] = ROcp25_36;
RxF1[2][1] = ROcp25_46;
RxF1[2][2] = ROcp25_56;
RxF1[2][3] = ROcp25_66;
RxF1[3][1] = S5;
RxF1[3][2] = ROcp25_85;
RxF1[3][3] = ROcp25_95;
VxF1[1] = qd[1];
VxF1[2] = qd[2];
VxF1[3] = qd[3];
OMxF1[1] = OMcp25_16;
OMxF1[2] = OMcp25_26;
OMxF1[3] = OMcp25_36;
ROcp26_25 = S4*S5;
ROcp26_35 = -C4*S5;
ROcp26_85 = -S4*C5;
ROcp26_95 = C4*C5;
ROcp26_16 = C5*C6;
ROcp26_26 = ROcp26_25*C6+C4*S6;
ROcp26_36 = ROcp26_35*C6+S4*S6;
ROcp26_46 = -C5*S6;
ROcp26_56 = -ROcp26_25*S6+C4*C6;
ROcp26_66 = -ROcp26_35*S6+S4*C6;
ROcp26_17 = ROcp26_16*C7-S5*S7;
ROcp26_27 = ROcp26_26*C7-ROcp26_85*S7;
ROcp26_37 = ROcp26_36*C7-ROcp26_95*S7;
ROcp26_77 = ROcp26_16*S7+S5*C7;
ROcp26_87 = ROcp26_26*S7+ROcp26_85*C7;
ROcp26_97 = ROcp26_36*S7+ROcp26_95*C7;
ROcp26_48 = ROcp26_46*C8+ROcp26_77*S8;
ROcp26_58 = ROcp26_56*C8+ROcp26_87*S8;
ROcp26_68 = ROcp26_66*C8+ROcp26_97*S8;
ROcp26_78 = -ROcp26_46*S8+ROcp26_77*C8;
ROcp26_88 = -ROcp26_56*S8+ROcp26_87*C8;
ROcp26_98 = -ROcp26_66*S8+ROcp26_97*C8;
ROcp26_19 = ROcp26_17*C9+ROcp26_48*S9;
ROcp26_29 = ROcp26_27*C9+ROcp26_58*S9;
ROcp26_39 = ROcp26_37*C9+ROcp26_68*S9;
ROcp26_49 = -ROcp26_17*S9+ROcp26_48*C9;
ROcp26_59 = -ROcp26_27*S9+ROcp26_58*C9;
ROcp26_69 = -ROcp26_37*S9+ROcp26_68*C9;
OMcp26_25 = qd[5]*C4;
OMcp26_35 = qd[5]*S4;
OMcp26_16 = qd[4]+qd[6]*S5;
OMcp26_26 = OMcp26_25+qd[6]*ROcp26_85;
OMcp26_36 = OMcp26_35+qd[6]*ROcp26_95;
RLcp26_17 = ROcp26_46*dpt[2][2];
RLcp26_27 = ROcp26_56*dpt[2][2];
RLcp26_37 = ROcp26_66*dpt[2][2];
POcp26_17 = q[1]+RLcp26_17;
POcp26_27 = q[2]+RLcp26_27;
POcp26_37 = q[3]+RLcp26_37;
OMcp26_17 = OMcp26_16+qd[7]*ROcp26_46;
OMcp26_27 = OMcp26_26+qd[7]*ROcp26_56;
OMcp26_37 = OMcp26_36+qd[7]*ROcp26_66;
ORcp26_17 = OMcp26_26*RLcp26_37-OMcp26_36*RLcp26_27;
ORcp26_27 = -OMcp26_16*RLcp26_37+OMcp26_36*RLcp26_17;
ORcp26_37 = OMcp26_16*RLcp26_27-OMcp26_26*RLcp26_17;
VIcp26_17 = qd[1]+ORcp26_17;
VIcp26_27 = qd[2]+ORcp26_27;
VIcp26_37 = qd[3]+ORcp26_37;
RLcp26_18 = ROcp26_46*dpt[2][6];
RLcp26_28 = ROcp26_56*dpt[2][6];
RLcp26_38 = ROcp26_66*dpt[2][6];
POcp26_18 = POcp26_17+RLcp26_18;
POcp26_28 = POcp26_27+RLcp26_28;
POcp26_38 = POcp26_37+RLcp26_38;
OMcp26_18 = OMcp26_17+qd[8]*ROcp26_17;
OMcp26_28 = OMcp26_27+qd[8]*ROcp26_27;
OMcp26_38 = OMcp26_37+qd[8]*ROcp26_37;
ORcp26_18 = OMcp26_27*RLcp26_38-OMcp26_37*RLcp26_28;
ORcp26_28 = -OMcp26_17*RLcp26_38+OMcp26_37*RLcp26_18;
ORcp26_38 = OMcp26_17*RLcp26_28-OMcp26_27*RLcp26_18;
VIcp26_18 = ORcp26_18+VIcp26_17;
VIcp26_28 = ORcp26_28+VIcp26_27;
VIcp26_38 = ORcp26_38+VIcp26_37;
RLcp26_19 = ROcp26_78*dpt[3][8];
RLcp26_29 = ROcp26_88*dpt[3][8];
RLcp26_39 = ROcp26_98*dpt[3][8];
POcp26_19 = POcp26_18+RLcp26_19;
POcp26_29 = POcp26_28+RLcp26_29;
POcp26_39 = POcp26_38+RLcp26_39;
OMcp26_19 = OMcp26_18+qd[9]*ROcp26_78;
OMcp26_29 = OMcp26_28+qd[9]*ROcp26_88;
OMcp26_39 = OMcp26_38+qd[9]*ROcp26_98;
ORcp26_19 = OMcp26_28*RLcp26_39-OMcp26_38*RLcp26_29;
ORcp26_29 = -OMcp26_18*RLcp26_39+OMcp26_38*RLcp26_19;
ORcp26_39 = OMcp26_18*RLcp26_29-OMcp26_28*RLcp26_19;
VIcp26_19 = ORcp26_19+VIcp26_18;
VIcp26_29 = ORcp26_29+VIcp26_28;
VIcp26_39 = ORcp26_39+VIcp26_38;
PxF2[1] = POcp26_19;
PxF2[2] = POcp26_29;
PxF2[3] = POcp26_39;
RxF2[1][1] = ROcp26_19;
RxF2[1][2] = ROcp26_29;
RxF2[1][3] = ROcp26_39;
RxF2[2][1] = ROcp26_49;
RxF2[2][2] = ROcp26_59;
RxF2[2][3] = ROcp26_69;
RxF2[3][1] = ROcp26_78;
RxF2[3][2] = ROcp26_88;
RxF2[3][3] = ROcp26_98;
VxF2[1] = VIcp26_19;
VxF2[2] = VIcp26_29;
VxF2[3] = VIcp26_39;
OMxF2[1] = OMcp26_19;
OMxF2[2] = OMcp26_29;
OMxF2[3] = OMcp26_39;
ROcp27_25 = S4*S5;
ROcp27_35 = -C4*S5;
ROcp27_85 = -S4*C5;
ROcp27_95 = C4*C5;
ROcp27_16 = C5*C6;
ROcp27_26 = ROcp27_25*C6+C4*S6;
ROcp27_36 = ROcp27_35*C6+S4*S6;
ROcp27_46 = -C5*S6;
ROcp27_56 = -ROcp27_25*S6+C4*C6;
ROcp27_66 = -ROcp27_35*S6+S4*C6;
ROcp27_17 = ROcp27_16*C7-S5*S7;
ROcp27_27 = ROcp27_26*C7-ROcp27_85*S7;
ROcp27_37 = ROcp27_36*C7-ROcp27_95*S7;
ROcp27_77 = ROcp27_16*S7+S5*C7;
ROcp27_87 = ROcp27_26*S7+ROcp27_85*C7;
ROcp27_97 = ROcp27_36*S7+ROcp27_95*C7;
ROcp27_48 = ROcp27_46*C8+ROcp27_77*S8;
ROcp27_58 = ROcp27_56*C8+ROcp27_87*S8;
ROcp27_68 = ROcp27_66*C8+ROcp27_97*S8;
ROcp27_78 = -ROcp27_46*S8+ROcp27_77*C8;
ROcp27_88 = -ROcp27_56*S8+ROcp27_87*C8;
ROcp27_98 = -ROcp27_66*S8+ROcp27_97*C8;
ROcp27_19 = ROcp27_17*C9+ROcp27_48*S9;
ROcp27_29 = ROcp27_27*C9+ROcp27_58*S9;
ROcp27_39 = ROcp27_37*C9+ROcp27_68*S9;
ROcp27_49 = -ROcp27_17*S9+ROcp27_48*C9;
ROcp27_59 = -ROcp27_27*S9+ROcp27_58*C9;
ROcp27_69 = -ROcp27_37*S9+ROcp27_68*C9;
ROcp27_110 = ROcp27_19*C10-ROcp27_78*S10;
ROcp27_210 = ROcp27_29*C10-ROcp27_88*S10;
ROcp27_310 = ROcp27_39*C10-ROcp27_98*S10;
ROcp27_710 = ROcp27_19*S10+ROcp27_78*C10;
ROcp27_810 = ROcp27_29*S10+ROcp27_88*C10;
ROcp27_910 = ROcp27_39*S10+ROcp27_98*C10;
OMcp27_25 = qd[5]*C4;
OMcp27_35 = qd[5]*S4;
OMcp27_16 = qd[4]+qd[6]*S5;
OMcp27_26 = OMcp27_25+qd[6]*ROcp27_85;
OMcp27_36 = OMcp27_35+qd[6]*ROcp27_95;
RLcp27_17 = ROcp27_46*dpt[2][2];
RLcp27_27 = ROcp27_56*dpt[2][2];
RLcp27_37 = ROcp27_66*dpt[2][2];
POcp27_17 = q[1]+RLcp27_17;
POcp27_27 = q[2]+RLcp27_27;
POcp27_37 = q[3]+RLcp27_37;
OMcp27_17 = OMcp27_16+qd[7]*ROcp27_46;
OMcp27_27 = OMcp27_26+qd[7]*ROcp27_56;
OMcp27_37 = OMcp27_36+qd[7]*ROcp27_66;
ORcp27_17 = OMcp27_26*RLcp27_37-OMcp27_36*RLcp27_27;
ORcp27_27 = -OMcp27_16*RLcp27_37+OMcp27_36*RLcp27_17;
ORcp27_37 = OMcp27_16*RLcp27_27-OMcp27_26*RLcp27_17;
VIcp27_17 = qd[1]+ORcp27_17;
VIcp27_27 = qd[2]+ORcp27_27;
VIcp27_37 = qd[3]+ORcp27_37;
RLcp27_18 = ROcp27_46*dpt[2][6];
RLcp27_28 = ROcp27_56*dpt[2][6];
RLcp27_38 = ROcp27_66*dpt[2][6];
POcp27_18 = POcp27_17+RLcp27_18;
POcp27_28 = POcp27_27+RLcp27_28;
POcp27_38 = POcp27_37+RLcp27_38;
OMcp27_18 = OMcp27_17+qd[8]*ROcp27_17;
OMcp27_28 = OMcp27_27+qd[8]*ROcp27_27;
OMcp27_38 = OMcp27_37+qd[8]*ROcp27_37;
ORcp27_18 = OMcp27_27*RLcp27_38-OMcp27_37*RLcp27_28;
ORcp27_28 = -OMcp27_17*RLcp27_38+OMcp27_37*RLcp27_18;
ORcp27_38 = OMcp27_17*RLcp27_28-OMcp27_27*RLcp27_18;
VIcp27_18 = ORcp27_18+VIcp27_17;
VIcp27_28 = ORcp27_28+VIcp27_27;
VIcp27_38 = ORcp27_38+VIcp27_37;
RLcp27_19 = ROcp27_78*dpt[3][8];
RLcp27_29 = ROcp27_88*dpt[3][8];
RLcp27_39 = ROcp27_98*dpt[3][8];
POcp27_19 = POcp27_18+RLcp27_19;
POcp27_29 = POcp27_28+RLcp27_29;
POcp27_39 = POcp27_38+RLcp27_39;
OMcp27_19 = OMcp27_18+qd[9]*ROcp27_78;
OMcp27_29 = OMcp27_28+qd[9]*ROcp27_88;
OMcp27_39 = OMcp27_38+qd[9]*ROcp27_98;
ORcp27_19 = OMcp27_28*RLcp27_39-OMcp27_38*RLcp27_29;
ORcp27_29 = -OMcp27_18*RLcp27_39+OMcp27_38*RLcp27_19;
ORcp27_39 = OMcp27_18*RLcp27_29-OMcp27_28*RLcp27_19;
VIcp27_19 = ORcp27_19+VIcp27_18;
VIcp27_29 = ORcp27_29+VIcp27_28;
VIcp27_39 = ORcp27_39+VIcp27_38;
RLcp27_110 = ROcp27_78*dpt[3][11];
RLcp27_210 = ROcp27_88*dpt[3][11];
RLcp27_310 = ROcp27_98*dpt[3][11];
POcp27_110 = POcp27_19+RLcp27_110;
POcp27_210 = POcp27_29+RLcp27_210;
POcp27_310 = POcp27_39+RLcp27_310;
OMcp27_110 = OMcp27_19+qd[10]*ROcp27_49;
OMcp27_210 = OMcp27_29+qd[10]*ROcp27_59;
OMcp27_310 = OMcp27_39+qd[10]*ROcp27_69;
ORcp27_110 = OMcp27_29*RLcp27_310-OMcp27_39*RLcp27_210;
ORcp27_210 = -OMcp27_19*RLcp27_310+OMcp27_39*RLcp27_110;
ORcp27_310 = OMcp27_19*RLcp27_210-OMcp27_29*RLcp27_110;
VIcp27_110 = ORcp27_110+VIcp27_19;
VIcp27_210 = ORcp27_210+VIcp27_29;
VIcp27_310 = ORcp27_310+VIcp27_39;
PxF3[1] = POcp27_110;
PxF3[2] = POcp27_210;
PxF3[3] = POcp27_310;
RxF3[1][1] = ROcp27_110;
RxF3[1][2] = ROcp27_210;
RxF3[1][3] = ROcp27_310;
RxF3[2][1] = ROcp27_49;
RxF3[2][2] = ROcp27_59;
RxF3[2][3] = ROcp27_69;
RxF3[3][1] = ROcp27_710;
RxF3[3][2] = ROcp27_810;
RxF3[3][3] = ROcp27_910;
VxF3[1] = VIcp27_110;
VxF3[2] = VIcp27_210;
VxF3[3] = VIcp27_310;
OMxF3[1] = OMcp27_110;
OMxF3[2] = OMcp27_210;
OMxF3[3] = OMcp27_310;
ROcp28_25 = S4*S5;
ROcp28_35 = -C4*S5;
ROcp28_85 = -S4*C5;
ROcp28_95 = C4*C5;
ROcp28_16 = C5*C6;
ROcp28_26 = ROcp28_25*C6+C4*S6;
ROcp28_36 = ROcp28_35*C6+S4*S6;
ROcp28_46 = -C5*S6;
ROcp28_56 = -ROcp28_25*S6+C4*C6;
ROcp28_66 = -ROcp28_35*S6+S4*C6;
ROcp28_17 = ROcp28_16*C7-S5*S7;
ROcp28_27 = ROcp28_26*C7-ROcp28_85*S7;
ROcp28_37 = ROcp28_36*C7-ROcp28_95*S7;
ROcp28_77 = ROcp28_16*S7+S5*C7;
ROcp28_87 = ROcp28_26*S7+ROcp28_85*C7;
ROcp28_97 = ROcp28_36*S7+ROcp28_95*C7;
ROcp28_48 = ROcp28_46*C8+ROcp28_77*S8;
ROcp28_58 = ROcp28_56*C8+ROcp28_87*S8;
ROcp28_68 = ROcp28_66*C8+ROcp28_97*S8;
ROcp28_78 = -ROcp28_46*S8+ROcp28_77*C8;
ROcp28_88 = -ROcp28_56*S8+ROcp28_87*C8;
ROcp28_98 = -ROcp28_66*S8+ROcp28_97*C8;
ROcp28_19 = ROcp28_17*C9+ROcp28_48*S9;
ROcp28_29 = ROcp28_27*C9+ROcp28_58*S9;
ROcp28_39 = ROcp28_37*C9+ROcp28_68*S9;
ROcp28_49 = -ROcp28_17*S9+ROcp28_48*C9;
ROcp28_59 = -ROcp28_27*S9+ROcp28_58*C9;
ROcp28_69 = -ROcp28_37*S9+ROcp28_68*C9;
ROcp28_110 = ROcp28_19*C10-ROcp28_78*S10;
ROcp28_210 = ROcp28_29*C10-ROcp28_88*S10;
ROcp28_310 = ROcp28_39*C10-ROcp28_98*S10;
ROcp28_710 = ROcp28_19*S10+ROcp28_78*C10;
ROcp28_810 = ROcp28_29*S10+ROcp28_88*C10;
ROcp28_910 = ROcp28_39*S10+ROcp28_98*C10;
ROcp28_411 = ROcp28_49*C11+ROcp28_710*S11;
ROcp28_511 = ROcp28_59*C11+ROcp28_810*S11;
ROcp28_611 = ROcp28_69*C11+ROcp28_910*S11;
ROcp28_711 = -ROcp28_49*S11+ROcp28_710*C11;
ROcp28_811 = -ROcp28_59*S11+ROcp28_810*C11;
ROcp28_911 = -ROcp28_69*S11+ROcp28_910*C11;
ROcp28_112 = ROcp28_110*C12-ROcp28_711*S12;
ROcp28_212 = ROcp28_210*C12-ROcp28_811*S12;
ROcp28_312 = ROcp28_310*C12-ROcp28_911*S12;
ROcp28_712 = ROcp28_110*S12+ROcp28_711*C12;
ROcp28_812 = ROcp28_210*S12+ROcp28_811*C12;
ROcp28_912 = ROcp28_310*S12+ROcp28_911*C12;
OMcp28_25 = qd[5]*C4;
OMcp28_35 = qd[5]*S4;
OMcp28_16 = qd[4]+qd[6]*S5;
OMcp28_26 = OMcp28_25+qd[6]*ROcp28_85;
OMcp28_36 = OMcp28_35+qd[6]*ROcp28_95;
RLcp28_17 = ROcp28_46*dpt[2][2];
RLcp28_27 = ROcp28_56*dpt[2][2];
RLcp28_37 = ROcp28_66*dpt[2][2];
POcp28_17 = q[1]+RLcp28_17;
POcp28_27 = q[2]+RLcp28_27;
POcp28_37 = q[3]+RLcp28_37;
OMcp28_17 = OMcp28_16+qd[7]*ROcp28_46;
OMcp28_27 = OMcp28_26+qd[7]*ROcp28_56;
OMcp28_37 = OMcp28_36+qd[7]*ROcp28_66;
ORcp28_17 = OMcp28_26*RLcp28_37-OMcp28_36*RLcp28_27;
ORcp28_27 = -OMcp28_16*RLcp28_37+OMcp28_36*RLcp28_17;
ORcp28_37 = OMcp28_16*RLcp28_27-OMcp28_26*RLcp28_17;
VIcp28_17 = qd[1]+ORcp28_17;
VIcp28_27 = qd[2]+ORcp28_27;
VIcp28_37 = qd[3]+ORcp28_37;
RLcp28_18 = ROcp28_46*dpt[2][6];
RLcp28_28 = ROcp28_56*dpt[2][6];
RLcp28_38 = ROcp28_66*dpt[2][6];
POcp28_18 = POcp28_17+RLcp28_18;
POcp28_28 = POcp28_27+RLcp28_28;
POcp28_38 = POcp28_37+RLcp28_38;
OMcp28_18 = OMcp28_17+qd[8]*ROcp28_17;
OMcp28_28 = OMcp28_27+qd[8]*ROcp28_27;
OMcp28_38 = OMcp28_37+qd[8]*ROcp28_37;
ORcp28_18 = OMcp28_27*RLcp28_38-OMcp28_37*RLcp28_28;
ORcp28_28 = -OMcp28_17*RLcp28_38+OMcp28_37*RLcp28_18;
ORcp28_38 = OMcp28_17*RLcp28_28-OMcp28_27*RLcp28_18;
VIcp28_18 = ORcp28_18+VIcp28_17;
VIcp28_28 = ORcp28_28+VIcp28_27;
VIcp28_38 = ORcp28_38+VIcp28_37;
RLcp28_19 = ROcp28_78*dpt[3][8];
RLcp28_29 = ROcp28_88*dpt[3][8];
RLcp28_39 = ROcp28_98*dpt[3][8];
POcp28_19 = POcp28_18+RLcp28_19;
POcp28_29 = POcp28_28+RLcp28_29;
POcp28_39 = POcp28_38+RLcp28_39;
OMcp28_19 = OMcp28_18+qd[9]*ROcp28_78;
OMcp28_29 = OMcp28_28+qd[9]*ROcp28_88;
OMcp28_39 = OMcp28_38+qd[9]*ROcp28_98;
ORcp28_19 = OMcp28_28*RLcp28_39-OMcp28_38*RLcp28_29;
ORcp28_29 = -OMcp28_18*RLcp28_39+OMcp28_38*RLcp28_19;
ORcp28_39 = OMcp28_18*RLcp28_29-OMcp28_28*RLcp28_19;
VIcp28_19 = ORcp28_19+VIcp28_18;
VIcp28_29 = ORcp28_29+VIcp28_28;
VIcp28_39 = ORcp28_39+VIcp28_38;
RLcp28_110 = ROcp28_78*dpt[3][11];
RLcp28_210 = ROcp28_88*dpt[3][11];
RLcp28_310 = ROcp28_98*dpt[3][11];
POcp28_110 = POcp28_19+RLcp28_110;
POcp28_210 = POcp28_29+RLcp28_210;
POcp28_310 = POcp28_39+RLcp28_310;
OMcp28_110 = OMcp28_19+qd[10]*ROcp28_49;
OMcp28_210 = OMcp28_29+qd[10]*ROcp28_59;
OMcp28_310 = OMcp28_39+qd[10]*ROcp28_69;
ORcp28_110 = OMcp28_29*RLcp28_310-OMcp28_39*RLcp28_210;
ORcp28_210 = -OMcp28_19*RLcp28_310+OMcp28_39*RLcp28_110;
ORcp28_310 = OMcp28_19*RLcp28_210-OMcp28_29*RLcp28_110;
VIcp28_110 = ORcp28_110+VIcp28_19;
VIcp28_210 = ORcp28_210+VIcp28_29;
VIcp28_310 = ORcp28_310+VIcp28_39;
RLcp28_111 = ROcp28_710*dpt[3][14];
RLcp28_211 = ROcp28_810*dpt[3][14];
RLcp28_311 = ROcp28_910*dpt[3][14];
POcp28_111 = POcp28_110+RLcp28_111;
POcp28_211 = POcp28_210+RLcp28_211;
POcp28_311 = POcp28_310+RLcp28_311;
OMcp28_111 = OMcp28_110+qd[11]*ROcp28_110;
OMcp28_211 = OMcp28_210+qd[11]*ROcp28_210;
OMcp28_311 = OMcp28_310+qd[11]*ROcp28_310;
ORcp28_111 = OMcp28_210*RLcp28_311-OMcp28_310*RLcp28_211;
ORcp28_211 = -OMcp28_110*RLcp28_311+OMcp28_310*RLcp28_111;
ORcp28_311 = OMcp28_110*RLcp28_211-OMcp28_210*RLcp28_111;
VIcp28_111 = ORcp28_111+VIcp28_110;
VIcp28_211 = ORcp28_211+VIcp28_210;
VIcp28_311 = ORcp28_311+VIcp28_310;
OMcp28_112 = OMcp28_111+qd[12]*ROcp28_411;
OMcp28_212 = OMcp28_211+qd[12]*ROcp28_511;
OMcp28_312 = OMcp28_311+qd[12]*ROcp28_611;
RLcp28_113 = ROcp28_712*dpt[3][18];
RLcp28_213 = ROcp28_812*dpt[3][18];
RLcp28_313 = ROcp28_912*dpt[3][18];
POcp28_113 = POcp28_111+RLcp28_113;
POcp28_213 = POcp28_211+RLcp28_213;
POcp28_313 = POcp28_311+RLcp28_313;
ORcp28_113 = OMcp28_212*RLcp28_313-OMcp28_312*RLcp28_213;
ORcp28_213 = -OMcp28_112*RLcp28_313+OMcp28_312*RLcp28_113;
ORcp28_313 = OMcp28_112*RLcp28_213-OMcp28_212*RLcp28_113;
VIcp28_113 = ORcp28_113+VIcp28_111;
VIcp28_213 = ORcp28_213+VIcp28_211;
VIcp28_313 = ORcp28_313+VIcp28_311;
PxF4[1] = POcp28_113;
PxF4[2] = POcp28_213;
PxF4[3] = POcp28_313;
RxF4[1][1] = ROcp28_112;
RxF4[1][2] = ROcp28_212;
RxF4[1][3] = ROcp28_312;
RxF4[2][1] = ROcp28_411;