1/* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/shell.hc,v 1.10 2017/01/12 14:46:44 masarati Exp $ */ 2/* 3 * MBDyn (C) is a multibody analysis code. 4 * http://www.mbdyn.org 5 * 6 * Copyright (C) 1996-2017 7 * 8 * Pierangelo Masarati <masarati@aero.polimi.it> 9 * Paolo Mantegazza <mantegazza@aero.polimi.it> 10 * 11 * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano 12 * via La Masa, 34 - 20156 Milano, Italy 13 * http://www.aero.polimi.it 14 * 15 * Changing this copyright notice is forbidden. 16 * 17 * This program is free software; you can redistribute it and/or modify 18 * it under the terms of the GNU General Public License as published by 19 * the Free Software Foundation (version 2 of the License). 20 * 21 * 22 * This program is distributed in the hope that it will be useful, 23 * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 * GNU General Public License for more details. 26 * 27 * You should have received a copy of the GNU General Public License 28 * along with this program; if not, write to the Free Software 29 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 30 */ 31 32#ifndef SHELL_HC 33#define SHELL_HC 34 35#if 0 36static inline void 37InsertMatrix(FullMatrixHandler & dest, 38 integer start_row, 39 integer start_col, 40 const FullMatrixHandler & source) 41{ 42 integer nr = source.iGetNumRows(); 43 integer nc = source.iGetNumCols(); 44 start_row--; 45 start_col--; 46 //TODO. error checking: check dimensions! 47 //check da fare: dest.iGetNumRows() <= nr+start_row 48 //check da fare: dest.iGetNumCols() <= nc+start_col 49 for (integer i = 1; i <= nr; i++) { 50 for (integer j = 1; j <= nc; j++) { 51 dest(i + start_row, j + start_col) = source(i, j); 52 } 53 } 54 return; 55} 56 57static inline void 58InsertMatrix(FullMatrixHandler & dest, 59 integer start_row, 60 integer start_col, 61 const Mat3x3 & source) 62{ 63 integer nr = 3; 64 integer nc = 3; 65 start_row--; 66 start_col--; 67 //TODO. error checking: check dimensions! 68 //check da fare: dest.iGetNumRows() <= nr+start_row 69 //check da fare: dest.iGetNumCols() <= nc+start_col 70 for (integer i = 1; i <= nr; i++) { 71 for (integer j = 1; j <= nc; j++) { 72 dest(i + start_row, j + start_col) = source(i, j); 73 } 74 } 75 return; 76} 77 78static inline void 79InsertMatrixT(FullMatrixHandler & dest, 80 integer start_row, 81 integer start_col, 82 const Mat3x3 & source) 83{ 84 integer nr = 3; 85 integer nc = 3; 86 start_row--; 87 start_col--; 88 //TODO. error checking: check dimensions! 89 //check da fare: dest.iGetNumRows() <= nr+start_row 90 //check da fare: dest.iGetNumCols() <= nc+start_col 91 for (integer i = 1; i <= nr; i++) { 92 for (integer j = 1; j <= nc; j++) { 93 dest(i + start_row, j + start_col) = source(j, i); 94 } 95 } 96 return; 97} 98 99static inline void 100InsertRowVector(FullMatrixHandler & dest, 101 integer start_row, 102 integer start_col, 103 const Vec3 & source) 104{ 105 integer nc = 3; 106 start_col--; 107 //TODO. error checking: check dimensions! 108 //check da fare: dest.iGetNumRows() <= start_row 109 //check da fare: dest.iGetNumCols() <= nc+start_col 110 for (integer j = 1; j <= nc; j++) { 111 dest(start_row, j + start_col) = source(j); 112 } 113 return; 114} 115 116static inline void 117CopyMatrixRow(FullMatrixHandler & dest, integer dest_row, 118 const FullMatrixHandler & source, integer source_row) 119{ 120 //TODO. error checking: check dimensions! 121 //check da fare: dest.iGetNumCols() == source.iGetNumCols() 122 //check da fare: 1 <= dest_row <= dest.iGetNumRows(); 123 //check da fare: 1 <= source_row <= source.iGetNumRows(); 124 integer nc = dest.iGetNumCols(); 125 for (integer i = 1; i <= nc; i++) { 126 dest(dest_row, i) = source(source_row, i); 127 } 128} 129 130static inline void 131CopyMatrixBlock(FullMatrixHandler & dest, integer dest_row, integer dest_col, 132 const FullMatrixHandler & source, 133 integer source_start_row, integer source_end_row, 134 integer source_start_col, integer source_end_col) 135{ 136 //TODO. error checking: check dimensions! 137 //check da fare: dest.iGetNumRows() >= dest_row + (source_end_row - source_start_row) 138 //check da fare: dest.iGetNumCols() >= dest_col + (source_end_col - source_start_col) 139 //check da fare: 1 <= source_start_row <= source.iGetNumRows(); 140 //check da fare: 1 <= source_start_col <= source.iGetNumRows(); 141 //check da fare: 1 <= source_end_row <= source.iGetNumRows(); 142 //check da fare: 1 <= source_end_col <= source.iGetNumRows(); 143 //check da fare: source_start_row <= source_end_row: 144 //check da fare: source_start_col <= source_end_col; 145 for (integer i = source_start_row; i <= source_end_row; i++) { 146 integer row = dest_row + (i - source_start_row); 147 for (integer ii = source_start_col; ii <= source_end_col; ii++) { 148 integer col = dest_col + (ii - source_start_col); 149 dest(row, col) = source(i, ii); 150 } 151 } 152} 153#endif 154 155#if 0 156static inline void 157InsertVector(MyVectorHandler & dest, 158 integer start_row, 159 const Vec3 & source) 160{ 161 integer nr = 3; 162 start_row--; 163 //TODO. error checking: check dimensions! 164 //check da fare: dest.iGetSize() <= nr+start_row 165 for (integer i = 1; i <= nr; i++) { 166 dest(i + start_row) = source(i); 167 } 168 return; 169} 170#endif 171 172static inline void 173AssembleVector(SubVectorHandler & dest, 174 integer start_row, 175 const MyVectorHandler & source, 176 const doublereal dCoef) 177{ 178 integer nr = source.iGetSize(); 179 start_row--; 180 //TODO. error checking: check dimensions! 181 //check da fare: dest.iGetSize() <= nr+start_row 182 for (integer i = 1; i <= nr; i++) { 183 dest(i + start_row) += source(i) * dCoef; 184 } 185 return; 186} 187 188#if 0 189static inline void 190AssembleMatrix(FullSubMatrixHandler & dest, 191 integer start_row, 192 integer start_col, 193 const FullMatrixHandler & source, 194 const doublereal dCoef) 195{ 196 integer nr = source.iGetNumRows(); 197 integer nc = source.iGetNumCols(); 198 start_row--; 199 start_col--; 200 201 //TODO. error checking: check dimensions! 202 //check da fare: dest.iGetNumRows() <= nr+start_row 203 //check da fare: dest.iGetNumCols() <= nc+start_col 204 for (integer ir = 1; ir <= nr; ir++) { 205 for (integer ic = 1; ic <= nc; ic++) { 206 dest.IncCoef(ir + start_row, ic + start_col, source(ir, ic) * dCoef); 207 // dest.ppdColsm1[ic + start_col][ir + start_row] += dCoef*source.ppdColsm1[ic][ir]; 208 } 209 } 210 211 return; 212} 213 214static inline void 215AssembleTransposeMatrix(FullSubMatrixHandler & dest, 216 integer start_row, 217 integer start_col, 218 const FullMatrixHandler & source, 219 const doublereal dCoef) 220{ 221 integer nr = source.iGetNumCols(); 222 integer nc = source.iGetNumRows(); 223 start_row--; 224 start_col--; 225 //TODO. error checking: check dimensions! 226 //check da fare: dest.iGetNumRows() <= nr+start_row 227 //check da fare: dest.iGetNumCols() <= nc+start_col 228 for (integer ir = 1; ir <= nr; ir++) { 229 for (integer ic = 1; ic <= nc; ic++) { 230 dest.IncCoef(ir + start_row, ic + start_col, source(ic, ir) * dCoef); 231 // dest.ppdColsm1[ic + start_col][ir + start_row] += dCoef*source.ppdColsm1[ir][ic]; 232 } 233 } 234 235 return; 236} 237#endif 238 239static inline void 240ExtractVec3(Vec3& dest, const MyVectorHandler & source, integer start_row) 241{ 242 //TODO. error checking: check dimensions! 243 //check da fare: source.iGetNumRows() >= start_row + 2 244 dest = Vec3(source(start_row), source(start_row + 1), source(start_row + 2)); 245} 246 247static inline void 248ExtractMat3x3(Mat3x3& dest, const FullMatrixHandler & source, 249 integer start_row, integer start_col) 250{ 251 //TODO. error checking: check dimensions! 252 //check da fare: source.iGetNumRows() >= start_row + 2 253 //check da fare: source.iGetNumCols() >= start_col + 2 254 dest = Mat3x3(source(start_row, start_col), source(start_row + 1, start_col), source(start_row + 2, start_col), 255 source(start_row, start_col + 1), source(start_row + 1, start_col + 1), source(start_row + 2, start_col + 1), 256 source(start_row, start_col + 2), source(start_row + 1, start_col + 2), source(start_row + 2, start_col + 2) 257 ); 258} 259 260static inline void 261RotateForward(MyVectorHandler & e, const Mat3x3& R) 262{ 263 //TODO. error checking: check dimensions! 264 //check da fare: e.iGetNumRows() == 12 265 Vec3 t, t1; 266 for (integer b = 0; b < 4; b++) { 267 ExtractVec3(t, e, 1 + b * 3); 268 t1 = R * t; 269 // InsertVector(e, 1 + b * 3, t1); 270 e.Put(1 + b * 3, t1); 271 } 272} 273 274static inline void 275RotateBackward(MyVectorHandler & e, const Mat3x3& R) 276{ 277 //TODO. error checking: check dimensions! 278 //check da fare: e.iGetNumRows() == 12 279 Vec3 t, t1; 280 for (integer b = 0; b < 4; b++) { 281 ExtractVec3(t, e, 1 + b * 3); 282 t1 = R.MulTV(t); 283 // InsertVector(e, 1 + b * 3, t1); 284 e.Put(1 + b * 3, t1); 285 } 286} 287 288static inline void 289RotateForward(FullMatrixHandler & C, const Mat3x3& R) 290{ 291 //TODO. error checking: check dimensions! 292 //check da fare: C.iGetNumRows() == 12 293 //check da fare: C.iGetNumCols() == 12 294 Mat3x3 m, m1; 295 for (integer rb = 0; rb < 4; rb++) { 296 for (integer rc = 0; rc < 4; rc++) { 297 ExtractMat3x3(m, C, 1 + rb * 3, 1 + rc * 3); 298 m1 = R * m.MulMT(R); 299 // InsertMatrix(C, 1 + rb * 3, 1 + rc * 3, m1); 300 C.Put(1 + rb * 3, 1 + rc * 3, m1); 301 } 302 } 303} 304 305static inline doublereal L1(const doublereal xi[2]) { 306 return 0.25 * (1. + xi[0]) * (1. + xi[1]); 307}; 308 309static inline doublereal L2(const doublereal xi[2]) { 310 return 0.25 * (1. - xi[0]) * (1. + xi[1]); 311}; 312 313static inline doublereal L3(const doublereal xi[2]) { 314 return 0.25 * (1. - xi[0]) * (1. - xi[1]); 315}; 316 317static inline doublereal L4(const doublereal xi[2]) { 318 return 0.25 * (1. + xi[0]) * (1. - xi[1]); 319}; 320 321typedef doublereal (*LI_Type)(const doublereal xi[2]); 322static LI_Type LI[4] = {&L1, &L2, &L3, &L4}; 323 324 325 326static inline doublereal 327L1_1(const doublereal xi[2]) 328{ 329 return 0.25 * (1. + xi[1]); 330} 331 332static inline doublereal 333L1_2(const doublereal xi[2]) 334{ 335 return 0.25 * (1. + xi[0]); 336} 337 338static inline doublereal 339L2_1(const doublereal xi[2]) 340{ 341 return -0.25 * (1. + xi[1]); 342} 343 344static inline doublereal 345L2_2(const doublereal xi[2]) 346{ 347 return 0.25 * (1. - xi[0]); 348} 349 350static inline doublereal 351L3_1(const doublereal xi[2]) 352{ 353 return -0.25 * (1. - xi[1]); 354} 355 356static inline doublereal 357L3_2(const doublereal xi[2]) 358{ 359 return -0.25 * (1. - xi[0]); 360} 361 362static inline doublereal 363L4_1(const doublereal xi[2]) 364{ 365 return 0.25 * (1. - xi[1]); 366} 367 368static inline doublereal 369L4_2(const doublereal xi[2]) 370{ 371 return -0.25 * (1. + xi[0]); 372} 373 374typedef doublereal (*LI_J_Type)(const doublereal xi[2]); 375static LI_J_Type LI_J[4][2] = { 376 {&L1_1, &L1_2}, 377 {&L2_1, &L2_2}, 378 {&L3_1, &L3_2}, 379 {&L4_1, &L4_2}, 380}; 381 382static Vec3 383Interp(const Vec3*const v, const doublereal xi[2]) 384{ 385 Vec3 r = v[0] * L1(xi) + 386 v[1] * L2(xi) + 387 v[2] * L3(xi) + 388 v[3] * L4(xi); 389 return r; 390} 391 392static Vec3 393InterpDeriv1(const Vec3*const v, const FullMatrixHandler & der_mat) 394{ 395 Vec3 r = v[0] * der_mat(1, 1) + 396 v[1] * der_mat(2, 1) + 397 v[2] * der_mat(3, 1) + 398 v[3] * der_mat(4, 1); 399 return r; 400} 401 402static Vec3 403InterpDeriv2(const Vec3*const v, const FullMatrixHandler & der_mat) 404{ 405 Vec3 r = v[0] * der_mat(1, 2) + 406 v[1] * der_mat(2, 2) + 407 v[2] * der_mat(3, 2) + 408 v[3] * der_mat(4, 2); 409 return r; 410} 411 412static void 413InterpDeriv(const Vec3*const v, 414 const FullMatrixHandler & der_mat, 415 Vec3 & der1, 416 Vec3 & der2) 417{ 418 der1 = InterpDeriv1(v, der_mat); 419 der2 = InterpDeriv2(v, der_mat); 420 return; 421} 422 423static Vec3 424InterpDeriv_xi1(const Vec3*const v, const doublereal xi[2]) 425{ 426 Vec3 r = v[0] * L1_1(xi) + 427 v[1] * L2_1(xi) + 428 v[2] * L3_1(xi) + 429 v[3] * L4_1(xi); 430 return r; 431} 432 433static Vec3 434InterpDeriv_xi2(const Vec3*const v, const doublereal xi[2]) 435{ 436 Vec3 r = v[0] * L1_2(xi) + 437 v[1] * L2_2(xi) + 438 v[2] * L3_2(xi) + 439 v[3] * L4_2(xi); 440 return r; 441} 442 443static void 444Inv2x2(const FullMatrixHandler& a, FullMatrixHandler & out) 445{ 446 //FIXME: Mettere controlli dimensioni matrici In/Out 447 //FIXME: mettere controllo det \neq 0 448 doublereal det = a(1, 1)*a(2, 2) - a(1, 2)*a(2, 1); 449 450 out(1, 1) = a(2, 2) / det; 451 out(1, 2) = -a(1, 2) / det; 452 out(2, 1) = -a(2, 1) / det; 453 out(2, 2) = a(1, 1) / det; 454 455 return; 456} 457 458static void 459Inv3x3(const FullMatrixHandler& a, FullMatrixHandler & out) 460{ 461 //FIXME: Mettere controlli dimensioni matrici In/Out 462 //FIXME: mettere controllo det \neq 0 463 doublereal det = 464 a(1, 1)*a(2, 2)*a(3, 3)-a(1, 1)*a(2, 3)*a(3, 2)-a(2, 1)*a(1, 2)*a(3, 3)+a(2, 1)*a(1, 3)*a(3, 2)+a(3, 1)*a(1, 2)*a(2, 3)-a(3, 1)*a(1, 3)*a(2, 2); 465 466 out(1, 1) = (a(2, 2)*a(3, 3)-a(2, 3)*a(3, 2))/det; 467 out(1, 2) = -(a(1, 2)*a(3, 3)+a(1, 3)*a(3, 2))/det; 468 out(1, 3) = (a(1, 2)*a(2, 3)-a(1, 3)*a(2, 2))/det; 469 470 out(2, 1) = -(a(2, 1)*a(3, 3)+a(2, 3)*a(3, 1))/det; 471 out(2, 2) = (a(1, 1)*a(3, 3)-a(1, 3)*a(3, 1))/det; 472 out(2, 3) = -(a(1, 1)*a(2, 3)+a(1, 3)*a(2, 1))/det; 473 474 out(3, 1) = (a(2, 1)*a(3, 2)-a(2, 2)*a(3, 1))/det; 475 out(3, 2) = -(a(1, 1)*a(3, 2)+a(1, 2)*a(3, 1))/det; 476 out(3, 3) = (a(1, 1)*a(2, 2)-a(1, 2)*a(2, 1))/det; 477 478 return; 479} 480 481static void 482Inv4x4(const FullMatrixHandler& a, FullMatrixHandler & out) 483{ 484 //FIXME: Mettere controlli dimensioni matrici In/Out 485 //FIXME: mettere controllo det \neq 0 486 doublereal det = 487 a(1, 1)*a(2, 2)*a(3, 3)*a(4, 4)-a(1, 1)*a(2, 2)*a(3, 4)*a(4, 3)-a(1, 1)*a(3, 2)*a(2, 3)*a(4, 4)+a(1, 1)*a(3, 2)*a(2, 4)*a(4, 3)+a(1, 1)*a(4, 2)*a(2, 3)*a(3, 4)-a(1, 1)*a(4, 2)*a(2, 4)*a(3, 3)-a(2, 1)*a(1, 2)*a(3, 3)*a(4, 4)+a(2, 1)*a(1, 2)*a(3, 4)*a(4, 3)+a(2, 1)*a(3, 2)*a(1, 3)*a(4, 4)-a(2, 1)*a(3, 2)*a(1, 4)*a(4, 3)-a(2, 1)*a(4, 2)*a(1, 3)*a(3, 4)+a(2, 1)*a(4, 2)*a(1, 4)*a(3, 3)+a(3, 1)*a(1, 2)*a(2, 3)*a(4, 4)-a(3, 1)*a(1, 2)*a(2, 4)*a(4, 3)-a(3, 1)*a(2, 2)*a(1, 3)*a(4, 4)+a(3, 1)*a(2, 2)*a(1, 4)*a(4, 3)+a(3, 1)*a(4, 2)*a(1, 3)*a(2, 4)-a(3, 1)*a(4, 2)*a(1, 4)*a(2, 3)-a(4, 1)*a(1, 2)*a(2, 3)*a(3, 4)+a(4, 1)*a(1, 2)*a(2, 4)*a(3, 3)+a(4, 1)*a(2, 2)*a(1, 3)*a(3, 4)-a(4, 1)*a(2, 2)*a(1, 4)*a(3, 3)-a(4, 1)*a(3, 2)*a(1, 3)*a(2, 4)+a(4, 1)*a(3, 2)*a(1, 4)*a(2, 3); 488 489 out(1, 1) = (a(2, 2)*a(3, 3)*a(4, 4)-a(2, 2)*a(3, 4)*a(4, 3)-a(3, 2)*a(2, 3)*a(4, 4)+a(3, 2)*a(2, 4)*a(4, 3)+a(4, 2)*a(2, 3)*a(3, 4)-a(4, 2)*a(2, 4)*a(3, 3))/det; 490 out(1, 2) = -(a(1, 2)*a(3, 3)*a(4, 4)-a(1, 2)*a(3, 4)*a(4, 3)-a(3, 2)*a(1, 3)*a(4, 4)+a(3, 2)*a(1, 4)*a(4, 3)+a(4, 2)*a(1, 3)*a(3, 4)-a(4, 2)*a(1, 4)*a(3, 3))/det; 491 out(1, 3) = (a(1, 2)*a(2, 3)*a(4, 4)-a(1, 2)*a(2, 4)*a(4, 3)-a(2, 2)*a(1, 3)*a(4, 4)+a(2, 2)*a(1, 4)*a(4, 3)+a(4, 2)*a(1, 3)*a(2, 4)-a(4, 2)*a(1, 4)*a(2, 3))/det; 492 out(1, 4) = -(a(1, 2)*a(2, 3)*a(3, 4)-a(1, 2)*a(2, 4)*a(3, 3)-a(2, 2)*a(1, 3)*a(3, 4)+a(2, 2)*a(1, 4)*a(3, 3)+a(3, 2)*a(1, 3)*a(2, 4)-a(3, 2)*a(1, 4)*a(2, 3))/det; 493 494 out(2, 1) = -(a(2, 1)*a(3, 3)*a(4, 4)-a(2, 1)*a(3, 4)*a(4, 3)-a(3, 1)*a(2, 3)*a(4, 4)+a(3, 1)*a(2, 4)*a(4, 3)+a(4, 1)*a(2, 3)*a(3, 4)-a(4, 1)*a(2, 4)*a(3, 3))/det; 495 out(2, 2) = (a(1, 1)*a(3, 3)*a(4, 4)-a(1, 1)*a(3, 4)*a(4, 3)-a(3, 1)*a(1, 3)*a(4, 4)+a(3, 1)*a(1, 4)*a(4, 3)+a(4, 1)*a(1, 3)*a(3, 4)-a(4, 1)*a(1, 4)*a(3, 3))/det; 496 out(2, 3) = -(a(1, 1)*a(2, 3)*a(4, 4)-a(1, 1)*a(2, 4)*a(4, 3)-a(2, 1)*a(1, 3)*a(4, 4)+a(2, 1)*a(1, 4)*a(4, 3)+a(4, 1)*a(1, 3)*a(2, 4)-a(4, 1)*a(1, 4)*a(2, 3))/det; 497 out(2, 4) = (a(1, 1)*a(2, 3)*a(3, 4)-a(1, 1)*a(2, 4)*a(3, 3)-a(2, 1)*a(1, 3)*a(3, 4)+a(2, 1)*a(1, 4)*a(3, 3)+a(3, 1)*a(1, 3)*a(2, 4)-a(3, 1)*a(1, 4)*a(2, 3))/det; 498 499 out(3, 1) = (a(2, 1)*a(3, 2)*a(4, 4)-a(2, 1)*a(3, 4)*a(4, 2)-a(3, 1)*a(2, 2)*a(4, 4)+a(3, 1)*a(2, 4)*a(4, 2)+a(4, 1)*a(2, 2)*a(3, 4)-a(4, 1)*a(2, 4)*a(3, 2))/det; 500 out(3, 2) = -(a(1, 1)*a(3, 2)*a(4, 4)-a(1, 1)*a(3, 4)*a(4, 2)-a(3, 1)*a(1, 2)*a(4, 4)+a(3, 1)*a(1, 4)*a(4, 2)+a(4, 1)*a(1, 2)*a(3, 4)-a(4, 1)*a(1, 4)*a(3, 2))/det; 501 out(3, 3) = (a(1, 1)*a(2, 2)*a(4, 4)-a(1, 1)*a(2, 4)*a(4, 2)-a(2, 1)*a(1, 2)*a(4, 4)+a(2, 1)*a(1, 4)*a(4, 2)+a(4, 1)*a(1, 2)*a(2, 4)-a(4, 1)*a(1, 4)*a(2, 2))/det; 502 out(3, 4) = -(a(1, 1)*a(2, 2)*a(3, 4)-a(1, 1)*a(2, 4)*a(3, 2)-a(2, 1)*a(1, 2)*a(3, 4)+a(2, 1)*a(1, 4)*a(3, 2)+a(3, 1)*a(1, 2)*a(2, 4)-a(3, 1)*a(1, 4)*a(2, 2))/det; 503 504 out(4, 1) = -(a(2, 1)*a(3, 2)*a(4, 3)-a(2, 1)*a(3, 3)*a(4, 2)-a(3, 1)*a(2, 2)*a(4, 3)+a(3, 1)*a(2, 3)*a(4, 2)+a(4, 1)*a(2, 2)*a(3, 3)-a(4, 1)*a(2, 3)*a(3, 2))/det; 505 out(4, 2) = (a(1, 1)*a(3, 2)*a(4, 3)-a(1, 1)*a(3, 3)*a(4, 2)-a(3, 1)*a(1, 2)*a(4, 3)+a(3, 1)*a(1, 3)*a(4, 2)+a(4, 1)*a(1, 2)*a(3, 3)-a(4, 1)*a(1, 3)*a(3, 2))/det; 506 out(4, 3) = -(a(1, 1)*a(2, 2)*a(4, 3)-a(1, 1)*a(2, 3)*a(4, 2)-a(2, 1)*a(1, 2)*a(4, 3)+a(2, 1)*a(1, 3)*a(4, 2)+a(4, 1)*a(1, 2)*a(2, 3)-a(4, 1)*a(1, 3)*a(2, 2))/det; 507 out(4, 4) = (a(1, 1)*a(2, 2)*a(3, 3)-a(1, 1)*a(2, 3)*a(3, 2)-a(2, 1)*a(1, 2)*a(3, 3)+a(2, 1)*a(1, 3)*a(3, 2)+a(3, 1)*a(1, 2)*a(2, 3)-a(3, 1)*a(1, 3)*a(2, 2))/det; 508 509 return; 510} 511 512static void 513InvBlockDiagonal3_2x3_2(const FullMatrixHandler& a, FullMatrixHandler & out) 514{ 515 //FIXME: Mettere controlli dimensioni matrici In/Out 516 //FIXME: mettere controllo det \neq 0 517 518 // 3x3 519 doublereal det = 520 a(1, 1)*a(2, 2)*a(3, 3)-a(1, 1)*a(2, 3)*a(3, 2)-a(2, 1)*a(1, 2)*a(3, 3)+a(2, 1)*a(1, 3)*a(3, 2)+a(3, 1)*a(1, 2)*a(2, 3)-a(3, 1)*a(1, 3)*a(2, 2); 521 522 out(1, 1) = (a(2, 2)*a(3, 3)-a(2, 3)*a(3, 2))/det; 523 out(1, 2) = -(a(1, 2)*a(3, 3)+a(1, 3)*a(3, 2))/det; 524 out(1, 3) = (a(1, 2)*a(2, 3)-a(1, 3)*a(2, 2))/det; 525 526 out(2, 1) = -(a(2, 1)*a(3, 3)+a(2, 3)*a(3, 1))/det; 527 out(2, 2) = (a(1, 1)*a(3, 3)-a(1, 3)*a(3, 1))/det; 528 out(2, 3) = -(a(1, 1)*a(2, 3)+a(1, 3)*a(2, 1))/det; 529 530 out(3, 1) = (a(2, 1)*a(3, 2)-a(2, 2)*a(3, 1))/det; 531 out(3, 2) = -(a(1, 1)*a(3, 2)+a(1, 2)*a(3, 1))/det; 532 out(3, 3) = (a(1, 1)*a(2, 2)-a(1, 2)*a(2, 1))/det; 533 534 535 // 2x2 536 det = a(4, 4)*a(5, 5) - a(4, 5)*a(5, 4); 537 538 out(4, 4) = a(5, 5) / det; 539 out(4, 5) = -a(4, 5) / det; 540 out(5, 4) = -a(5, 4) / det; 541 out(5, 5) = a(4, 4) / det; 542 543 return; 544} 545 546static void 547InvBlockDiagonal4_2x4_2(const FullMatrixHandler& a, FullMatrixHandler & out) 548{ 549 //FIXME: Mettere controlli dimensioni matrici In/Out 550 //FIXME: mettere controllo det \neq 0 551 552 // 4x4 553 doublereal det = 554 a(1, 1)*a(2, 2)*a(3, 3)*a(4, 4)-a(1, 1)*a(2, 2)*a(3, 4)*a(4, 3)-a(1, 1)*a(3, 2)*a(2, 3)*a(4, 4)+a(1, 1)*a(3, 2)*a(2, 4)*a(4, 3)+a(1, 1)*a(4, 2)*a(2, 3)*a(3, 4)-a(1, 1)*a(4, 2)*a(2, 4)*a(3, 3)-a(2, 1)*a(1, 2)*a(3, 3)*a(4, 4)+a(2, 1)*a(1, 2)*a(3, 4)*a(4, 3)+a(2, 1)*a(3, 2)*a(1, 3)*a(4, 4)-a(2, 1)*a(3, 2)*a(1, 4)*a(4, 3)-a(2, 1)*a(4, 2)*a(1, 3)*a(3, 4)+a(2, 1)*a(4, 2)*a(1, 4)*a(3, 3)+a(3, 1)*a(1, 2)*a(2, 3)*a(4, 4)-a(3, 1)*a(1, 2)*a(2, 4)*a(4, 3)-a(3, 1)*a(2, 2)*a(1, 3)*a(4, 4)+a(3, 1)*a(2, 2)*a(1, 4)*a(4, 3)+a(3, 1)*a(4, 2)*a(1, 3)*a(2, 4)-a(3, 1)*a(4, 2)*a(1, 4)*a(2, 3)-a(4, 1)*a(1, 2)*a(2, 3)*a(3, 4)+a(4, 1)*a(1, 2)*a(2, 4)*a(3, 3)+a(4, 1)*a(2, 2)*a(1, 3)*a(3, 4)-a(4, 1)*a(2, 2)*a(1, 4)*a(3, 3)-a(4, 1)*a(3, 2)*a(1, 3)*a(2, 4)+a(4, 1)*a(3, 2)*a(1, 4)*a(2, 3); 555 556 out(1, 1) = (a(2, 2)*a(3, 3)*a(4, 4)-a(2, 2)*a(3, 4)*a(4, 3)-a(3, 2)*a(2, 3)*a(4, 4)+a(3, 2)*a(2, 4)*a(4, 3)+a(4, 2)*a(2, 3)*a(3, 4)-a(4, 2)*a(2, 4)*a(3, 3))/det; 557 out(1, 2) = -(a(1, 2)*a(3, 3)*a(4, 4)-a(1, 2)*a(3, 4)*a(4, 3)-a(3, 2)*a(1, 3)*a(4, 4)+a(3, 2)*a(1, 4)*a(4, 3)+a(4, 2)*a(1, 3)*a(3, 4)-a(4, 2)*a(1, 4)*a(3, 3))/det; 558 out(1, 3) = (a(1, 2)*a(2, 3)*a(4, 4)-a(1, 2)*a(2, 4)*a(4, 3)-a(2, 2)*a(1, 3)*a(4, 4)+a(2, 2)*a(1, 4)*a(4, 3)+a(4, 2)*a(1, 3)*a(2, 4)-a(4, 2)*a(1, 4)*a(2, 3))/det; 559 out(1, 4) = -(a(1, 2)*a(2, 3)*a(3, 4)-a(1, 2)*a(2, 4)*a(3, 3)-a(2, 2)*a(1, 3)*a(3, 4)+a(2, 2)*a(1, 4)*a(3, 3)+a(3, 2)*a(1, 3)*a(2, 4)-a(3, 2)*a(1, 4)*a(2, 3))/det; 560 561 out(2, 1) = -(a(2, 1)*a(3, 3)*a(4, 4)-a(2, 1)*a(3, 4)*a(4, 3)-a(3, 1)*a(2, 3)*a(4, 4)+a(3, 1)*a(2, 4)*a(4, 3)+a(4, 1)*a(2, 3)*a(3, 4)-a(4, 1)*a(2, 4)*a(3, 3))/det; 562 out(2, 2) = (a(1, 1)*a(3, 3)*a(4, 4)-a(1, 1)*a(3, 4)*a(4, 3)-a(3, 1)*a(1, 3)*a(4, 4)+a(3, 1)*a(1, 4)*a(4, 3)+a(4, 1)*a(1, 3)*a(3, 4)-a(4, 1)*a(1, 4)*a(3, 3))/det; 563 out(2, 3) = -(a(1, 1)*a(2, 3)*a(4, 4)-a(1, 1)*a(2, 4)*a(4, 3)-a(2, 1)*a(1, 3)*a(4, 4)+a(2, 1)*a(1, 4)*a(4, 3)+a(4, 1)*a(1, 3)*a(2, 4)-a(4, 1)*a(1, 4)*a(2, 3))/det; 564 out(2, 4) = (a(1, 1)*a(2, 3)*a(3, 4)-a(1, 1)*a(2, 4)*a(3, 3)-a(2, 1)*a(1, 3)*a(3, 4)+a(2, 1)*a(1, 4)*a(3, 3)+a(3, 1)*a(1, 3)*a(2, 4)-a(3, 1)*a(1, 4)*a(2, 3))/det; 565 566 out(3, 1) = (a(2, 1)*a(3, 2)*a(4, 4)-a(2, 1)*a(3, 4)*a(4, 2)-a(3, 1)*a(2, 2)*a(4, 4)+a(3, 1)*a(2, 4)*a(4, 2)+a(4, 1)*a(2, 2)*a(3, 4)-a(4, 1)*a(2, 4)*a(3, 2))/det; 567 out(3, 2) = -(a(1, 1)*a(3, 2)*a(4, 4)-a(1, 1)*a(3, 4)*a(4, 2)-a(3, 1)*a(1, 2)*a(4, 4)+a(3, 1)*a(1, 4)*a(4, 2)+a(4, 1)*a(1, 2)*a(3, 4)-a(4, 1)*a(1, 4)*a(3, 2))/det; 568 out(3, 3) = (a(1, 1)*a(2, 2)*a(4, 4)-a(1, 1)*a(2, 4)*a(4, 2)-a(2, 1)*a(1, 2)*a(4, 4)+a(2, 1)*a(1, 4)*a(4, 2)+a(4, 1)*a(1, 2)*a(2, 4)-a(4, 1)*a(1, 4)*a(2, 2))/det; 569 out(3, 4) = -(a(1, 1)*a(2, 2)*a(3, 4)-a(1, 1)*a(2, 4)*a(3, 2)-a(2, 1)*a(1, 2)*a(3, 4)+a(2, 1)*a(1, 4)*a(3, 2)+a(3, 1)*a(1, 2)*a(2, 4)-a(3, 1)*a(1, 4)*a(2, 2))/det; 570 571 out(4, 1) = -(a(2, 1)*a(3, 2)*a(4, 3)-a(2, 1)*a(3, 3)*a(4, 2)-a(3, 1)*a(2, 2)*a(4, 3)+a(3, 1)*a(2, 3)*a(4, 2)+a(4, 1)*a(2, 2)*a(3, 3)-a(4, 1)*a(2, 3)*a(3, 2))/det; 572 out(4, 2) = (a(1, 1)*a(3, 2)*a(4, 3)-a(1, 1)*a(3, 3)*a(4, 2)-a(3, 1)*a(1, 2)*a(4, 3)+a(3, 1)*a(1, 3)*a(4, 2)+a(4, 1)*a(1, 2)*a(3, 3)-a(4, 1)*a(1, 3)*a(3, 2))/det; 573 out(4, 3) = -(a(1, 1)*a(2, 2)*a(4, 3)-a(1, 1)*a(2, 3)*a(4, 2)-a(2, 1)*a(1, 2)*a(4, 3)+a(2, 1)*a(1, 3)*a(4, 2)+a(4, 1)*a(1, 2)*a(2, 3)-a(4, 1)*a(1, 3)*a(2, 2))/det; 574 out(4, 4) = (a(1, 1)*a(2, 2)*a(3, 3)-a(1, 1)*a(2, 3)*a(3, 2)-a(2, 1)*a(1, 2)*a(3, 3)+a(2, 1)*a(1, 3)*a(3, 2)+a(3, 1)*a(1, 2)*a(2, 3)-a(3, 1)*a(1, 3)*a(2, 2))/det; 575 576 577 // 2x2 578 det = a(5, 5)*a(6, 6) - a(5, 6)*a(6, 5); 579 580 out(5, 5) = a(6, 6) / det; 581 out(5, 6) = -a(5, 6) / det; 582 out(6, 5) = -a(6, 5) / det; 583 out(6, 6) = a(5, 5) / det; 584 585 return; 586} 587 588#endif // SHELL_HC 589 590// vim:ft=c 591