File: calculateTransformations.c1 /* 2 * Academic License - for use in teaching, academic research, and meeting 3 * course requirements at degree granting institutions only. Not for 4 * government, commercial, or other organizational use. 5 * 6 * calculateTransformations.c 7 * 8 * Code generation for function 'calculateTransformations' 9 * 10 */ 11 12 /* Include files */ 13 #include "rt_nonfinite.h" 14 #include "calculateTransformations.h" 15 #include "rot2euler.h" 16 #include "calculateTransformations_data.h" 17 18 /* Variable Definitions */ 19 static emlrtRSInfo emlrtRSI = { 29, "calculateTransformations", 20 "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m" 21 }; 22 23 static emlrtRSInfo b_emlrtRSI = { 42, "calculateTransformations", 24 "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m" 25 }; 26 27 static emlrtRSInfo c_emlrtRSI = { 45, "calculateTransformations", 28 "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m" 29 }; 30 31 static emlrtRSInfo d_emlrtRSI = { 48, "calculateTransformations", 32 "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m" 33 }; 34 35 static emlrtRSInfo e_emlrtRSI = { 74, "calculateTransformations", 36 "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m" 37 }; 38 39 static emlrtBCInfo emlrtBCI = { 1, 8, 16, 42, "g_joints", 40 "calculateModuleTransformations", 41 "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateModuleTransformations.m", 42 0 }; 43 44 static emlrtBCInfo b_emlrtBCI = { 1, 8, 30, 45, "g_joints", 45 "calculateJointTransformations", 46 "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateJointTransformations.m", 47 0 }; 48 49 static emlrtBCInfo c_emlrtBCI = { 1, 8, 29, 40, "g_joints", 50 "calculateJointTransformations", 51 "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateJointTransformations.m", 52 0 }; 53 54 /* Function Definitions */ 55 void calculateTransformations(const emlrtStack *sp, const real_T q1[5], const 56 real_T q2[5], const real_T eta_marker[6], const real_T g_marker2front[16], 57 real_T controlCenterLink, real_T g_If[16], real_T R_If[9], real_T eta_f[6], 58 real_T g_Ib[16], real_T R_Ib[9], real_T eta_b[6], real_T q[8], real_T 59 g_joints_home[144], real_T g_moduleCM[144], real_T g_thrusters[112]) 60 { 61 real_T cphi; 62 real_T sphi; 63 real_T cth; 64 real_T sth; 65 real_T cpsi; 66 real_T spsi; 67 real_T b_cpsi[16]; 68 int32_T k; 69 static const int8_T iv0[4] = { 0, 0, 0, 1 }; 70 71 int32_T i0; 72 int32_T i1; 73 real_T b_g_If[9]; 74 real_T b_g_joints_home[3]; 75 real_T g_joints[128]; 76 int32_T i; 77 boolean_T b_bool; 78 static const char_T joint_ax[8] = { 'Z', 'Y', 'Z', 'Y', 'Z', 'Y', 'Z', 'Y' }; 79 80 real_T cos_angle; 81 real_T sin_angle; 82 real_T g_front2back[16]; 83 static const int8_T iv1[3] = { 0, 1, 0 }; 84 85 static const real_T joint_tr[27] = { 0.62, 0.1, 0.59, 0.1, 0.8, 0.1, 0.59, 0.1, 86 0.37, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 87 0.0, 0.0, 0.0, 0.0 }; 88 89 int8_T I[9]; 90 real_T g_center2back[16]; 91 static const int8_T iv2[3] = { 0, 0, 1 }; 92 93 static const real_T g_ne[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 94 0.0, 1.0, 0.0, 0.37, 0.0, 0.0, 1.0 }; 95 96 static const int8_T iv3[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; 97 98 static const real_T module_cm[27] = { 0.31, 0.05, 0.295, 0.05, 0.4, 0.05, 99 0.295, 0.05, 0.185, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; 101 102 static const char_T thruster_ax[7] = { 'Z', 'Y', 'Z', 'X', 'X', 'Y', 'Z' }; 103 104 static const real_T rotZ[9] = { 6.123233995736766E-17, 1.0, 0.0, -1.0, 105 6.123233995736766E-17, 0.0, 0.0, 0.0, 1.0 }; 106 107 static const real_T thruster_pos[21] = { 0.24, 0.35000000000000003, 0.24, 0.53, 108 0.53, 0.24, 0.35000000000000003, 0.0, 0.0, 0.0, 0.15, -0.15, 0.0, 0.0, 0.0, 109 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; 110 111 static const real_T rotY[9] = { 6.123233995736766E-17, 0.0, 1.0, 0.0, 1.0, 0.0, 112 -1.0, 0.0, 6.123233995736766E-17 }; 113 114 static const int8_T iv4[7] = { 3, 3, 5, 5, 5, 7, 7 }; 115 116 emlrtStack st; 117 st.prev = sp; 118 st.tls = sp->tls; 119 120 /* [EELY] Specific code for Eely */ 121 /* Measured: Position and Euler angles for the markers, eta_marker */ 122 /* R = Rzyx(phi,theta,psi) computes the Euler angle */ 123 /* rotation matrix R in SO(3) using the zyx convention */ 124 /* */ 125 /* Author: Thor I. Fossen */ 126 /* Date: 14th June 2001 */ 127 /* Revisions: */ 128 /* ________________________________________________________________ */ 129 /* */ 130 /* MSS GNC is a Matlab toolbox for guidance, navigation and control. */ 131 /* The toolbox is part of the Marine Systems Simulator (MSS). */ 132 /* */ 133 /* Copyright (C) 2008 Thor I. Fossen and Tristan Perez */ 134 /* */ 135 /* This program is free software: you can redistribute it and/or modify */ 136 /* it under the terms of the GNU General Public License as published by */ 137 /* the Free Software Foundation, either version 3 of the License, or */ 138 /* (at your option) any later version. */ 139 /* */ 140 /* This program is distributed in the hope that it will be useful, but */ 141 /* WITHOUT ANY WARRANTY; without even the implied warranty of */ 142 /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */ 143 /* GNU General Public License for more details. */ 144 /* */ 145 /* You should have received a copy of the GNU General Public License */ 146 /* along with this program. If not, see <http://www.gnu.org/licenses/>. */ 147 /* */ 148 /* E-mail: contact@marinecontrol.org */ 149 /* URL: <http://www.marinecontrol.org> */ 150 cphi = muDoubleScalarCos(eta_marker[3]); 151 sphi = muDoubleScalarSin(eta_marker[3]); 152 cth = muDoubleScalarCos(eta_marker[4]); 153 sth = muDoubleScalarSin(eta_marker[4]); 154 cpsi = muDoubleScalarCos(eta_marker[5]); 155 spsi = muDoubleScalarSin(eta_marker[5]); 156 b_cpsi[0] = cpsi * cth; 157 b_cpsi[4] = -spsi * cphi + cpsi * sth * sphi; 158 b_cpsi[8] = spsi * sphi + cpsi * cphi * sth; 159 b_cpsi[1] = spsi * cth; 160 b_cpsi[5] = cpsi * cphi + sphi * sth * spsi; 161 b_cpsi[9] = -cpsi * sphi + sth * spsi * cphi; 162 b_cpsi[2] = -sth; 163 b_cpsi[6] = cth * sphi; 164 b_cpsi[10] = cth * cphi; 165 for (k = 0; k < 3; k++) { 166 b_cpsi[12 + k] = eta_marker[k]; 167 } 168 169 for (k = 0; k < 4; k++) { 170 b_cpsi[3 + (k << 2)] = iv0[k]; 171 } 172 173 for (k = 0; k < 4; k++) { 174 for (i0 = 0; i0 < 4; i0++) { 175 g_If[k + (i0 << 2)] = 0.0; 176 for (i1 = 0; i1 < 4; i1++) { 177 g_If[k + (i0 << 2)] += b_cpsi[k + (i1 << 2)] * g_marker2front[i1 + (i0 << 178 2)]; 179 } 180 } 181 } 182 183 for (k = 0; k < 3; k++) { 184 for (i0 = 0; i0 < 3; i0++) { 185 R_If[i0 + 3 * k] = g_If[i0 + (k << 2)]; 186 b_g_If[i0 + 3 * k] = g_If[i0 + (k << 2)]; 187 } 188 } 189 190 st.site = &emlrtRSI; 191 rot2euler(&st, b_g_If, b_g_joints_home); 192 for (k = 0; k < 3; k++) { 193 eta_f[k] = g_If[12 + k]; 194 eta_f[k + 3] = b_g_joints_home[k]; 195 } 196 197 /* Code to convert the joint angle inputs q1 and q2 from the Eely robot */ 198 /* to the definition used in this code by reorganizing the vector elements. */ 199 q[0] = q2[3] * 3.1415926535897931 / 180.0; 200 q[1] = q1[3] * 3.1415926535897931 / 180.0; 201 q[2] = q2[2] * 3.1415926535897931 / 180.0; 202 q[3] = q1[2] * 3.1415926535897931 / 180.0; 203 q[4] = q2[1] * 3.1415926535897931 / 180.0; 204 q[5] = q1[1] * 3.1415926535897931 / 180.0; 205 q[6] = q2[4] * 3.1415926535897931 / 180.0; 206 q[7] = q1[4] * 3.1415926535897931 / 180.0; 207 208 /* % TRANSFORMATIONS */ 209 /* Calculate the sequential transformations from base frame to the joint */ 210 /* frames */ 211 st.site = &b_emlrtRSI; 212 213 /* Calculates the transformation matrices from */ 214 /* the USM base frame, F_b = F_0, to the joint frames F_i (i=1..n) */ 215 /* From the tail module and forward */ 216 /* JOINT FRAMES */ 217 memset(&g_joints[0], 0, sizeof(real_T) << 7); 218 memset(&g_joints_home[0], 0, 144U * sizeof(real_T)); 219 220 /* size = num_joint+1 to include F_e */ 221 i = 0; 222 while (i < 8) { 223 b_bool = false; 224 if (joint_ax[i] != 'Y') { 225 } else { 226 b_bool = true; 227 } 228 229 if (b_bool) { 230 cos_angle = muDoubleScalarCos(q[i]); 231 sin_angle = muDoubleScalarSin(q[i]); 232 g_front2back[0] = cos_angle; 233 g_front2back[4] = 0.0; 234 g_front2back[8] = sin_angle; 235 g_front2back[2] = -sin_angle; 236 g_front2back[6] = 0.0; 237 g_front2back[10] = cos_angle; 238 for (k = 0; k < 3; k++) { 239 g_front2back[1 + (k << 2)] = iv1[k]; 240 g_front2back[12 + k] = joint_tr[i + 9 * k]; 241 } 242 243 for (k = 0; k < 4; k++) { 244 g_front2back[3 + (k << 2)] = iv0[k]; 245 } 246 247 for (k = 0; k < 9; k++) { 248 I[k] = 0; 249 } 250 251 for (k = 0; k < 3; k++) { 252 I[k + 3 * k] = 1; 253 } 254 255 for (k = 0; k < 3; k++) { 256 for (i0 = 0; i0 < 3; i0++) { 257 g_center2back[i0 + (k << 2)] = I[i0 + 3 * k]; 258 } 259 260 g_center2back[12 + k] = joint_tr[i + 9 * k]; 261 } 262 263 for (k = 0; k < 4; k++) { 264 g_center2back[3 + (k << 2)] = iv0[k]; 265 } 266 } else { 267 b_bool = false; 268 if (joint_ax[i] != 'Z') { 269 } else { 270 b_bool = true; 271 } 272 273 if (b_bool) { 274 cos_angle = muDoubleScalarCos(q[i]); 275 sin_angle = muDoubleScalarSin(q[i]); 276 g_front2back[0] = cos_angle; 277 g_front2back[4] = -sin_angle; 278 g_front2back[8] = 0.0; 279 g_front2back[1] = sin_angle; 280 g_front2back[5] = cos_angle; 281 g_front2back[9] = 0.0; 282 for (k = 0; k < 3; k++) { 283 g_front2back[2 + (k << 2)] = iv2[k]; 284 g_front2back[12 + k] = joint_tr[i + 9 * k]; 285 } 286 287 for (k = 0; k < 4; k++) { 288 g_front2back[3 + (k << 2)] = iv0[k]; 289 } 290 291 for (k = 0; k < 9; k++) { 292 I[k] = 0; 293 } 294 295 for (k = 0; k < 3; k++) { 296 I[k + 3 * k] = 1; 297 } 298 299 for (k = 0; k < 3; k++) { 300 for (i0 = 0; i0 < 3; i0++) { 301 g_center2back[i0 + (k << 2)] = I[i0 + 3 * k]; 302 } 303 304 g_center2back[12 + k] = joint_tr[i + 9 * k]; 305 } 306 307 for (k = 0; k < 4; k++) { 308 g_center2back[3 + (k << 2)] = iv0[k]; 309 } 310 } else { 311 memset(&g_front2back[0], 0, sizeof(real_T) << 4); 312 for (k = 0; k < 4; k++) { 313 g_front2back[k + (k << 2)] = 1.0; 314 } 315 316 memset(&g_center2back[0], 0, sizeof(real_T) << 4); 317 for (k = 0; k < 4; k++) { 318 g_center2back[k + (k << 2)] = 1.0; 319 } 320 } 321 } 322 323 if (1 + i == 1) { 324 for (k = 0; k < 4; k++) { 325 for (i0 = 0; i0 < 4; i0++) { 326 g_joints[(i0 + (k << 2)) + (i << 4)] = g_front2back[i0 + (k << 2)]; 327 g_joints_home[(i0 + (k << 2)) + (i << 4)] = g_center2back[i0 + (k << 2)]; 328 } 329 } 330 } else { 331 k = i; 332 if (!(k >= 1)) { 333 emlrtDynamicBoundsCheckR2012b(0, 1, 8, &c_emlrtBCI, &st); 334 } 335 336 for (k = 0; k < 4; k++) { 337 for (i0 = 0; i0 < 4; i0++) { 338 b_cpsi[k + (i0 << 2)] = 0.0; 339 for (i1 = 0; i1 < 4; i1++) { 340 b_cpsi[k + (i0 << 2)] += g_joints[(k + (i1 << 2)) + ((i - 1) << 4)] * 341 g_front2back[i1 + (i0 << 2)]; 342 } 343 } 344 } 345 346 for (k = 0; k < 4; k++) { 347 for (i0 = 0; i0 < 4; i0++) { 348 g_joints[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)]; 349 } 350 } 351 352 k = i; 353 if (!(k >= 1)) { 354 emlrtDynamicBoundsCheckR2012b(0, 1, 8, &b_emlrtBCI, &st); 355 } 356 357 for (k = 0; k < 4; k++) { 358 for (i0 = 0; i0 < 4; i0++) { 359 g_joints_home[(k + (i0 << 2)) + (i << 4)] = 0.0; 360 for (i1 = 0; i1 < 4; i1++) { 361 g_joints_home[(k + (i0 << 2)) + (i << 4)] += g_joints[(k + (i1 << 2)) 362 + ((i - 1) << 4)] * g_center2back[i1 + (i0 << 2)]; 363 } 364 } 365 } 366 } 367 368 i++; 369 if (*emlrtBreakCheckR2012bFlagVar != 0) { 370 emlrtBreakCheckR2012b(&st); 371 } 372 } 373 374 for (k = 0; k < 4; k++) { 375 for (i0 = 0; i0 < 4; i0++) { 376 g_joints_home[128 + (k + (i0 << 2))] = 0.0; 377 for (i1 = 0; i1 < 4; i1++) { 378 g_joints_home[128 + (k + (i0 << 2))] += g_joints[112 + (k + (i1 << 2))] * 379 g_ne[i1 + (i0 << 2)]; 380 } 381 } 382 } 383 384 /* Calculate transformations for the CM of each module */ 385 st.site = &c_emlrtRSI; 386 387 /* Calculating the transformations from the USM base frame, F_b = F_0, */ 388 /* to the MODULE CENTER OF MASS FRAMES, F_CM_i */ 389 /* From the tail module and forward */ 390 memset(&g_moduleCM[0], 0, 144U * sizeof(real_T)); 391 i = 0; 392 while (i < 9) { 393 for (k = 0; k < 3; k++) { 394 for (i0 = 0; i0 < 3; i0++) { 395 g_front2back[i0 + (k << 2)] = iv3[i0 + 3 * k]; 396 } 397 398 g_front2back[12 + k] = module_cm[i + 9 * k]; 399 } 400 401 for (k = 0; k < 4; k++) { 402 g_front2back[3 + (k << 2)] = iv0[k]; 403 } 404 405 if (1 + i == 1) { 406 for (k = 0; k < 4; k++) { 407 for (i0 = 0; i0 < 4; i0++) { 408 g_moduleCM[(i0 + (k << 2)) + (i << 4)] = g_front2back[i0 + (k << 2)]; 409 } 410 } 411 } else { 412 k = i; 413 if (!(k >= 1)) { 414 emlrtDynamicBoundsCheckR2012b(0, 1, 8, &emlrtBCI, &st); 415 } 416 417 for (k = 0; k < 4; k++) { 418 for (i0 = 0; i0 < 4; i0++) { 419 g_moduleCM[(k + (i0 << 2)) + (i << 4)] = 0.0; 420 for (i1 = 0; i1 < 4; i1++) { 421 g_moduleCM[(k + (i0 << 2)) + (i << 4)] += g_joints[(k + (i1 << 2)) + 422 ((i - 1) << 4)] * g_front2back[i1 + (i0 << 2)]; 423 } 424 } 425 } 426 } 427 428 i++; 429 if (*emlrtBreakCheckR2012bFlagVar != 0) { 430 emlrtBreakCheckR2012b(&st); 431 } 432 } 433 434 /* Calculate transformations for the thrusters */ 435 st.site = &d_emlrtRSI; 436 437 /* Calculating the transformations from the back frame to the thruster */ 438 /* frames */ 439 /* From the tail module and forward */ 440 /* THRUSTER FRAMES */ 441 memset(&g_thrusters[0], 0, 112U * sizeof(real_T)); 442 i = 0; 443 while (i < 7) { 444 b_bool = false; 445 if (thruster_ax[i] != 'Y') { 446 } else { 447 b_bool = true; 448 } 449 450 if (b_bool) { 451 k = 0; 452 } else { 453 b_bool = false; 454 if (thruster_ax[i] != 'Z') { 455 } else { 456 b_bool = true; 457 } 458 459 if (b_bool) { 460 k = 1; 461 } else { 462 k = -1; 463 } 464 } 465 466 switch (k) { 467 case 0: 468 for (k = 0; k < 3; k++) { 469 for (i0 = 0; i0 < 3; i0++) { 470 g_front2back[i0 + (k << 2)] = rotZ[i0 + 3 * k]; 471 } 472 473 g_front2back[12 + k] = thruster_pos[i + 7 * k]; 474 } 475 476 for (k = 0; k < 4; k++) { 477 g_front2back[3 + (k << 2)] = iv0[k]; 478 } 479 break; 480 481 case 1: 482 for (k = 0; k < 3; k++) { 483 for (i0 = 0; i0 < 3; i0++) { 484 g_front2back[i0 + (k << 2)] = rotY[i0 + 3 * k]; 485 } 486 487 g_front2back[12 + k] = thruster_pos[i + 7 * k]; 488 } 489 490 for (k = 0; k < 4; k++) { 491 g_front2back[3 + (k << 2)] = iv0[k]; 492 } 493 break; 494 495 default: 496 memset(&g_front2back[0], 0, sizeof(real_T) << 4); 497 for (k = 0; k < 4; k++) { 498 g_front2back[k + (k << 2)] = 1.0; 499 } 500 break; 501 } 502 503 for (k = 0; k < 4; k++) { 504 for (i0 = 0; i0 < 4; i0++) { 505 g_center2back[i0 + (k << 2)] = g_joints[(i0 + (k << 2)) + ((iv4[i] - 2) << 506 4)]; 507 } 508 } 509 510 for (k = 0; k < 4; k++) { 511 for (i0 = 0; i0 < 4; i0++) { 512 g_thrusters[(k + (i0 << 2)) + (i << 4)] = 0.0; 513 for (i1 = 0; i1 < 4; i1++) { 514 g_thrusters[(k + (i0 << 2)) + (i << 4)] += g_center2back[k + (i1 << 2)] 515 * g_front2back[i1 + (i0 << 2)]; 516 } 517 } 518 } 519 520 i++; 521 if (*emlrtBreakCheckR2012bFlagVar != 0) { 522 emlrtBreakCheckR2012b(&st); 523 } 524 } 525 526 /* Calculate the transformation from the front end to the back end */ 527 for (k = 0; k < 3; k++) { 528 for (i0 = 0; i0 < 3; i0++) { 529 b_g_If[i0 + 3 * k] = -g_joints_home[128 + (k + (i0 << 2))]; 530 } 531 } 532 533 for (k = 0; k < 3; k++) { 534 b_g_joints_home[k] = 0.0; 535 for (i0 = 0; i0 < 3; i0++) { 536 g_front2back[i0 + (k << 2)] = g_joints_home[128 + (k + (i0 << 2))]; 537 b_g_joints_home[k] += b_g_If[k + 3 * i0] * g_joints_home[140 + i0]; 538 } 539 540 g_front2back[12 + k] = b_g_joints_home[k]; 541 } 542 543 for (k = 0; k < 4; k++) { 544 g_front2back[3 + (k << 2)] = iv0[k]; 545 } 546 547 /* Transformation from inertial frame to base frame */ 548 if (controlCenterLink != 0.0) { 549 /* Calculate the transformation from the back end to the center module */ 550 /* (module number 5) */ 551 for (k = 0; k < 3; k++) { 552 for (i0 = 0; i0 < 3; i0++) { 553 b_g_If[i0 + 3 * k] = -g_moduleCM[64 + (k + (i0 << 2))]; 554 } 555 } 556 557 for (k = 0; k < 3; k++) { 558 b_g_joints_home[k] = 0.0; 559 for (i0 = 0; i0 < 3; i0++) { 560 g_center2back[i0 + (k << 2)] = g_moduleCM[64 + (k + (i0 << 2))]; 561 b_g_joints_home[k] += b_g_If[k + 3 * i0] * g_moduleCM[76 + i0]; 562 } 563 564 g_center2back[12 + k] = b_g_joints_home[k]; 565 } 566 567 for (k = 0; k < 4; k++) { 568 g_center2back[3 + (k << 2)] = iv0[k]; 569 for (i0 = 0; i0 < 4; i0++) { 570 b_cpsi[k + (i0 << 2)] = 0.0; 571 for (i1 = 0; i1 < 4; i1++) { 572 b_cpsi[k + (i0 << 2)] += g_If[k + (i1 << 2)] * g_front2back[i1 + (i0 << 573 2)]; 574 } 575 } 576 577 for (i0 = 0; i0 < 4; i0++) { 578 g_Ib[k + (i0 << 2)] = 0.0; 579 for (i1 = 0; i1 < 4; i1++) { 580 g_Ib[k + (i0 << 2)] += b_cpsi[k + (i1 << 2)] * g_moduleCM[64 + (i1 + 581 (i0 << 2))]; 582 } 583 } 584 } 585 586 i = 0; 587 while (i < 7) { 588 for (k = 0; k < 4; k++) { 589 for (i0 = 0; i0 < 4; i0++) { 590 b_cpsi[k + (i0 << 2)] = 0.0; 591 for (i1 = 0; i1 < 4; i1++) { 592 b_cpsi[k + (i0 << 2)] += g_center2back[k + (i1 << 2)] * g_thrusters 593 [(i1 + (i0 << 2)) + (i << 4)]; 594 } 595 } 596 } 597 598 for (k = 0; k < 4; k++) { 599 for (i0 = 0; i0 < 4; i0++) { 600 g_thrusters[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)]; 601 } 602 } 603 604 i++; 605 if (*emlrtBreakCheckR2012bFlagVar != 0) { 606 emlrtBreakCheckR2012b(sp); 607 } 608 } 609 610 i = 0; 611 while (i < 9) { 612 for (k = 0; k < 4; k++) { 613 for (i0 = 0; i0 < 4; i0++) { 614 b_cpsi[k + (i0 << 2)] = 0.0; 615 for (i1 = 0; i1 < 4; i1++) { 616 b_cpsi[k + (i0 << 2)] += g_center2back[k + (i1 << 2)] * g_moduleCM 617 [(i1 + (i0 << 2)) + (i << 4)]; 618 } 619 } 620 } 621 622 for (k = 0; k < 4; k++) { 623 for (i0 = 0; i0 < 4; i0++) { 624 g_moduleCM[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)]; 625 } 626 } 627 628 i++; 629 if (*emlrtBreakCheckR2012bFlagVar != 0) { 630 emlrtBreakCheckR2012b(sp); 631 } 632 } 633 } else { 634 for (k = 0; k < 4; k++) { 635 for (i0 = 0; i0 < 4; i0++) { 636 g_Ib[k + (i0 << 2)] = 0.0; 637 for (i1 = 0; i1 < 4; i1++) { 638 g_Ib[k + (i0 << 2)] += g_If[k + (i1 << 2)] * g_front2back[i1 + (i0 << 639 2)]; 640 } 641 } 642 } 643 } 644 645 for (k = 0; k < 3; k++) { 646 for (i0 = 0; i0 < 3; i0++) { 647 R_Ib[i0 + 3 * k] = g_Ib[i0 + (k << 2)]; 648 b_g_If[i0 + 3 * k] = g_Ib[i0 + (k << 2)]; 649 } 650 } 651 652 st.site = &e_emlrtRSI; 653 rot2euler(&st, b_g_If, b_g_joints_home); 654 for (k = 0; k < 3; k++) { 655 eta_b[k] = g_Ib[12 + k]; 656 eta_b[k + 3] = b_g_joints_home[k]; 657 } 658 } 659 660 /* End of code generation (calculateTransformations.c) */ 661 |