Geant4 Cross Reference |
1 // Copyright (C) 2010, Guy Barrand. All rights 2 // See the file tools.license for terms. 3 4 #ifndef tools_MATCOM 5 #define tools_MATCOM 6 7 /* NOTE : bof, no big improvement. 8 #include <cstring> //memcpy 9 inline void vec_copy(double* a_to,const double 10 ::memcpy(a_to,a_from,a_number*sizeof(double) 11 } 12 13 template <class T> 14 inline void vec_copy(T* a_to,const T* a_from,u 15 T* pto = (T*)a_to; 16 T* pfm = (T*)a_from; 17 for(unsigned int i=0;i<a_number;i++,pto++,pf 18 } 19 */ 20 21 /*NOTE : bof, no big improvement. 22 #include <Accelerate/Accelerate.h> 23 24 inline void vec_add(const float* a_1,const flo 25 ::vDSP_vadd(a_1,1,a_2,1,a_res,1,a_number); 26 } 27 inline void vec_sub(const float* a_1,const flo 28 ::vDSP_vsub(a_1,1,a_2,1,a_res,1,a_number); 29 } 30 */ 31 32 /* 33 template <class T> 34 inline void vec_add(const T* a_1,const T* a_2, 35 T* p1 = (T*)a_1; 36 T* p2 = (T*)a_2; 37 T* pr = (T*)a_res; 38 for(unsigned int i=0;i<a_number;i++,p1++,p2+ 39 } 40 41 template <class T> 42 inline void vec_sub(const T* a_1,const T* a_2, 43 T* p1 = (T*)a_1; 44 T* p2 = (T*)a_2; 45 T* pr = (T*)a_res; 46 for(unsigned int i=0;i<a_number;i++,p1++,p2+ 47 } 48 */ 49 50 #include "../eqT" 51 52 #include <cstddef> //size_t 53 54 // common code to class mat and nmat. 55 56 #define TOOLS_MATCOM \ 57 protected:\ 58 static T zero() {return T();}\ 59 static T one() {return T(1);}\ 60 static T minus_one() {return T(-1);}\ 61 static T two() {return T(2);}\ 62 public:\ 63 typedef T elem_t;\ 64 typedef unsigned int size_type;\ 65 public:\ 66 unsigned int rows() const {return dimension( 67 unsigned int cols() const {return dimension( 68 \ 69 void set_value(unsigned int aR,unsigned int 70 m_vec[aR + aC * dimension()] = a_value;\ 71 }\ 72 \ 73 const T& value(unsigned int aR,unsigned int 74 return m_vec[aR + aC * dimension()];\ 75 }\ 76 \ 77 T value(unsigned int aR,unsigned int aC) { \ 78 return m_vec[aR + aC * dimension()];\ 79 }\ 80 \ 81 void set_matrix(const TOOLS_MAT_CLASS& a_m){ 82 _copy(a_m.m_vec);\ 83 }\ 84 \ 85 void set_constant(const T& a_v){\ 86 for(unsigned int i=0;i<dim2();i++) m_vec[i 87 }\ 88 void set_zero(){\ 89 set_constant(zero());\ 90 }\ 91 void set_identity() {\ 92 set_zero();\ 93 for(unsigned int i=0;i<dimension();i++) m_ 94 }\ 95 void set_diagonal(const T& a_s) {\ 96 set_zero();\ 97 for(unsigned int i=0;i<dimension();i++) m_ 98 }\ 99 typedef T (*func)(const T&);\ 100 void apply_func(func a_func) {\ 101 T* pos = m_vec;\ 102 for(unsigned int i=0;i<dim2();i++,pos++) * 103 }\ 104 template <class RANDOM>\ 105 void set_random(RANDOM& a_random) {\ 106 for(unsigned int i=0;i<dim2();i++) m_vec[i 107 }\ 108 template <class RANDOM>\ 109 void set_symmetric_random(RANDOM& a_random) 110 unsigned int _D = dimension();\ 111 {for(unsigned int r=0;r<_D;r++) set_value(r 112 T rd;\ 113 {for(unsigned int r=0;r<_D;r++) {\ 114 for(unsigned int c=(r+1);c<_D;c++) {\ 115 rd = a_random.shoot();\ 116 set_value(r,c,rd);\ 117 set_value(c,r,rd);\ 118 }\ 119 }}\ 120 }\ 121 template <class RANDOM>\ 122 void set_antisymmetric_random(RANDOM& a_rand 123 unsigned int _D = dimension();\ 124 {for(unsigned int r=0;r<_D;r++) set_value(r 125 T rd;\ 126 {for(unsigned int r=0;r<_D;r++) {\ 127 for(unsigned int c=(r+1);c<_D;c++) {\ 128 rd = a_random.shoot();\ 129 set_value(r,c,rd);\ 130 set_value(c,r,minus_one()*rd);\ 131 }\ 132 }}\ 133 }\ 134 public:\ 135 template <class ARRAY>\ 136 bool mul_array(ARRAY& a_array,T a_tmp[]) con 137 /* a_array = this *= a_array */\ 138 unsigned int _dim = dimension();\ 139 T* pos = a_tmp;\ 140 for(unsigned int r=0;r<_dim;r++,pos++) {\ 141 *pos = T();\ 142 for(unsigned int c=0;c<_dim;c++) *pos += 143 }\ 144 {for(unsigned int i=0;i<_dim;i++) a_array[i 145 return true;\ 146 }\ 147 template <class VEC>\ 148 bool mul_vec(VEC& a_vec,T a_tmp[]) const {\ 149 /* a_vec = this *= a_vec */\ 150 unsigned int _dim = dimension();\ 151 if(a_vec.dimension()!=_dim) return false;\ 152 T* pos = a_tmp;\ 153 for(unsigned int r=0;r<_dim;r++,pos++) {\ 154 *pos = T();\ 155 for(unsigned int c=0;c<_dim;c++) *pos += 156 }\ 157 {for(unsigned int i=0;i<_dim;i++) a_vec[i] 158 return true;\ 159 }\ 160 template <class VEC>\ 161 bool mul_vec(VEC& a_vec) const {\ 162 T* _tmp = new T[dimension()];\ 163 bool status = mul_vec(a_vec,_tmp);\ 164 delete [] _tmp;\ 165 return status;\ 166 }\ 167 \ 168 bool mul_array(T a_vec[],T a_tmp[]) const {\ 169 /* a_vec = this *= a_vec */\ 170 unsigned int _dim = dimension();\ 171 T* pos = a_tmp;\ 172 for(unsigned int r=0;r<_dim;r++,pos++) {\ 173 *pos = T();\ 174 for(unsigned int c=0;c<_dim;c++) *pos += 175 }\ 176 {for(unsigned int i=0;i<_dim;i++) a_vec[i] 177 return true;\ 178 }\ 179 bool mul_array(T a_vec[]) const {\ 180 T* _tmp = new T[dimension()];\ 181 bool status = mul_array(a_vec,_tmp);\ 182 delete [] _tmp;\ 183 return status;\ 184 }\ 185 \ 186 void mul_mtx(const TOOLS_MAT_CLASS& a_m) {\ 187 _mul_mtx(a_m.m_vec);\ 188 }\ 189 void mul_mtx(const TOOLS_MAT_CLASS& a_m,T a_ 190 _mul_mtx(a_m.m_vec,a_tmp);\ 191 }\ 192 void left_mul_mtx(const TOOLS_MAT_CLASS& a_m 193 /* this = a_m * this :*/\ 194 _left_mul_mtx(a_m.m_vec);\ 195 }\ 196 bool equal(const TOOLS_MAT_CLASS& a_m) const 197 if(&a_m==this) return true;\ 198 for(unsigned int i=0;i<dim2();i++) {\ 199 if(m_vec[i]!=a_m.m_vec[i]) return false; 200 }\ 201 return true;\ 202 }\ 203 \ 204 bool equal(const TOOLS_MAT_CLASS& a_m,const 205 if(&a_m==this) return true;\ 206 T* tp = (T*)m_vec;\ 207 T* mp = (T*)a_m.m_vec;\ 208 for(unsigned int i=0;i<dim2();i++,tp++,mp+ 209 T diff = (*tp) - (*mp);\ 210 if(diff<zero()) diff *= minus_one();\ 211 if(diff>=a_prec) return false;\ 212 }\ 213 return true;\ 214 }\ 215 \ 216 template <class PREC>\ 217 bool equal_prec(const TOOLS_MAT_CLASS& a_m,c 218 if(&a_m==this) return true;\ 219 T* tp = (T*)m_vec;\ 220 T* mp = (T*)a_m.m_vec;\ 221 for(unsigned int i=0;i<dim2();i++,tp++,mp+ 222 T diff = (*tp) - (*mp);\ 223 if(a_fabs(diff)>=a_prec) return false;\ 224 }\ 225 return true;\ 226 }\ 227 \ 228 void mx_diff(const TOOLS_MAT_CLASS& a_m,T& a 229 T* tp = (T*)m_vec;\ 230 T* mp = (T*)a_m.m_vec;\ 231 a_mx_diff = (*tp) - (*mp);\ 232 if(a_mx_diff<zero()) a_mx_diff *= minus_on 233 for(unsigned int i=0;i<dim2();i++,tp++,mp+ 234 T diff = (*tp) - (*mp);\ 235 if(diff<zero()) diff *= minus_one();\ 236 a_mx_diff = (diff>a_mx_diff?diff:a_mx_di 237 }\ 238 }\ 239 \ 240 bool to_rm_is_proportional(const TOOLS_MAT_C 241 if(&a_m==this) {a_factor=one();return true 242 /* If true, then : a_m = a_factor * this.* 243 a_factor = zero();\ 244 T* tp = (T*)m_vec;\ 245 T* mp = (T*)a_m.m_vec;\ 246 bool first = true;\ 247 for(unsigned int i=0;i<dim2();i++,tp++,mp+ 248 if( ((*tp)==zero()) && ((*mp)==ze 249 continue;\ 250 /*} else if( ((*tp)!=zero()) && ((*mp)== 251 return false;*/\ 252 } else if( ((*tp)==zero()) && ((*mp)!=ze 253 return false;\ 254 } else {\ 255 if(first) {\ 256 a_factor = (*mp)/(*tp);\ 257 first = false;\ 258 } else {\ 259 T diff = (*tp)*a_factor - (*mp);\ 260 if(diff<zero()) diff *= minus_one(); 261 if(diff>=a_prec) return false;\ 262 }\ 263 }\ 264 }\ 265 return true;\ 266 }\ 267 \ 268 public:\ 269 bool is_proportional(const TOOLS_MAT_CLASS& 270 /* If true, then : a_right = a_factor * th 271 if(this==&a_right) {a_factor=T(1);return t 272 a_factor = zero();\ 273 if(dimension()!=a_right.dimension()) retur 274 T* lp = (T*)m_vec;\ 275 T* rp = (T*)a_right.m_vec;\ 276 bool first = true;\ 277 size_t _data_size = data_size();\ 278 for(size_t i=0;i<_data_size;i++,lp++,rp++) 279 if(*lp==zero()) {\ 280 if(*rp==zero()) continue;\ 281 return false;\ 282 }\ 283 if(first) {\ 284 a_factor = (*rp)/(*lp);\ 285 first = false;\ 286 continue;\ 287 }\ 288 if((*lp)*a_factor!=(*rp)) return false;\ 289 }\ 290 return true;\ 291 }\ 292 \ 293 template <class PREC>\ 294 bool is_proportional_prec(const TOOLS_MAT_CL 295 /* If true, then : a_right = a_factor * th 296 if(this==&a_right) {a_factor=T(1);return t 297 a_factor = zero();\ 298 if(dimension()!=a_right.dimension()) retur 299 T* lp = (T*)m_vec;\ 300 T* rp = (T*)a_right.m_vec;\ 301 bool first = true;\ 302 size_t _data_size = data_size();\ 303 for(size_t i=0;i<_data_size;i++,lp++,rp++) 304 if(is_zero(*lp,a_prec,a_fabs)) {\ 305 if(is_zero(*rp,a_prec,a_fabs)) continu 306 return false;\ 307 }\ 308 if(first) {\ 309 a_factor = (*rp)/(*lp);\ 310 first = false;\ 311 continue;\ 312 }\ 313 if(!numbers_are_equal((*lp)*a_factor,*rp 314 }\ 315 return true;\ 316 }\ 317 \ 318 bool is_diagonal() const {\ 319 unsigned int _D = dimension();\ 320 for(unsigned int r=0;r<_D;r++) {\ 321 for(unsigned int c=0;c<_D;c++) {\ 322 if(c!=r) {if(value(r,c)!=zero()) retur 323 }\ 324 }\ 325 return true;\ 326 }\ 327 \ 328 template <class PREC>\ 329 bool is_diagonal_prec(const PREC& a_prec,PRE 330 unsigned int _D = dimension();\ 331 for(unsigned int r=0;r<_D;r++) {\ 332 for(unsigned int c=0;c<_D;c++) {\ 333 if(c!=r) {if(!is_zero(value(r,c),a_pre 334 }\ 335 }\ 336 return true;\ 337 }\ 338 \ 339 bool is_identity() const {\ 340 unsigned int _D = dimension();\ 341 {for(unsigned int r=0;r<_D;r++) {\ 342 if(value(r,r)!=one()) return false;\ 343 }}\ 344 {for(unsigned int r=0;r<_D;r++) {\ 345 for(unsigned int c=0;c<_D;c++) {\ 346 if(c!=r) {if(value(r,c)!=zero()) retur 347 }\ 348 }}\ 349 return true;\ 350 }\ 351 \ 352 template <class PREC>\ 353 bool is_identity_prec(const PREC& a_prec,PRE 354 unsigned int _D = dimension();\ 355 {for(unsigned int r=0;r<_D;r++) {\ 356 if(!numbers_are_equal(value(r,r),one(),a 357 }}\ 358 {for(unsigned int r=0;r<_D;r++) {\ 359 for(unsigned int c=0;c<_D;c++) {\ 360 if(c!=r) {if(!is_zero(value(r,c),a_pre 361 }\ 362 }}\ 363 return true;\ 364 }\ 365 \ 366 const T* data() const {return m_vec;}\ 367 unsigned int size() const {return dim2();}\ 368 unsigned int data_size() const {return dim2( 369 \ 370 T trace() const {\ 371 T _value = zero();\ 372 unsigned int _D = dimension();\ 373 for(unsigned int c=0;c<_D;c++) _value += m 374 return _value;\ 375 }\ 376 \ 377 void transpose() {\ 378 unsigned int _D = dimension();\ 379 for(unsigned int r=0;r<_D;r++) {\ 380 for(unsigned int c=(r+1);c<_D;c++) {\ 381 T vrc = value(r,c);\ 382 T vcr = value(c,r);\ 383 set_value(r,c,vcr);\ 384 set_value(c,r,vrc);\ 385 }\ 386 }\ 387 }\ 388 \ 389 void multiply(const T& a_T) {\ 390 for(unsigned int i=0;i<dim2();i++) m_vec[i 391 }\ 392 \ 393 bool is_symmetric() const {\ 394 unsigned int _D = dimension();\ 395 for(unsigned int r=0;r<_D;r++) {\ 396 for(unsigned int c=(r+1);c<_D;c++) {\ 397 if(value(r,c)!=value(c,r)) return fals 398 }\ 399 }\ 400 return true;\ 401 }\ 402 \ 403 template <class PREC>\ 404 bool is_symmetric_prec(const PREC& a_prec,PR 405 unsigned int _D = dimension();\ 406 for(unsigned int r=0;r<_D;r++) {\ 407 for(unsigned int c=(r+1);c<_D;c++) {\ 408 T diff = value(r,c)-value(c,r);\ 409 if(a_fabs(diff)>=a_prec) return false; 410 }\ 411 }\ 412 return true;\ 413 }\ 414 \ 415 bool is_antisymmetric() const {\ 416 unsigned int _D = dimension();\ 417 {for(unsigned int r=0;r<_D;r++) {\ 418 if(value(r,r)!=zero()) return false;\ 419 }}\ 420 for(unsigned int r=0;r<_D;r++) {\ 421 for(unsigned int c=(r+1);c<_D;c++) {\ 422 if(value(r,c)!=minus_one()*value(c,r)) 423 }\ 424 }\ 425 return true;\ 426 }\ 427 \ 428 template <class PREC>\ 429 bool is_antisymmetric_prec(const PREC& a_pre 430 unsigned int _D = dimension();\ 431 {for(unsigned int r=0;r<_D;r++) {\ 432 if(a_fabs(value(r,r))>=a_prec) return fa 433 }}\ 434 for(unsigned int r=0;r<_D;r++) {\ 435 for(unsigned int c=(r+1);c<_D;c++) {\ 436 T diff = value(r,c)-minus_one()*value( 437 if(a_fabs(diff)>=a_prec) return false; 438 }\ 439 }\ 440 return true;\ 441 }\ 442 \ 443 void symmetric_part(TOOLS_MAT_CLASS& a_res) 444 a_res = *this;\ 445 a_res.transpose();\ 446 a_res += *this;\ 447 a_res.multiply(one()/two());\ 448 }\ 449 \ 450 void antisymmetric_part(TOOLS_MAT_CLASS& a_r 451 a_res = *this;\ 452 a_res.transpose();\ 453 a_res.multiply(minus_one());\ 454 a_res += *this;\ 455 a_res.multiply(one()/two());\ 456 }\ 457 \ 458 template <class PREC>\ 459 bool is_block_UL_DR(const PREC& a_prec,PREC( 460 /* Look if even dim matrix is of the form 461 462 unsigned int _D = dimension();\ 463 unsigned int _D_2 = _D/2;\ 464 if(2*_D_2!=_D) return false;\ 465 for(unsigned int r=0;r<_D_2;r++) {\ 466 for(unsigned int c=_D_2;c<_D;c++) {\ 467 if(a_fabs(value(r,c))>=a_prec) return 468 }\ 469 }\ 470 for(unsigned int r=_D_2;r<_D;r++) {\ 471 for(unsigned int c=0;c<_D_2;c++) {\ 472 if(a_fabs(value(r,c))>=a_prec) return 473 }\ 474 }\ 475 return true;\ 476 }\ 477 \ 478 template <class PREC>\ 479 bool is_block_UR_DL(const PREC& a_prec,PREC( 480 /* Look if even dim matrix is of the form 481 482 unsigned int _D = dimension();\ 483 unsigned int _D_2 = _D/2;\ 484 if(2*_D_2!=_D) return false;\ 485 for(unsigned int r=0;r<_D_2;r++) {\ 486 for(unsigned int c=0;c<_D_2;c++) {\ 487 if(a_fabs(value(r,c))>=a_prec) return 488 }\ 489 }\ 490 for(unsigned int r=_D_2;r<_D;r++) {\ 491 for(unsigned int c=_D_2;c<_D;c++) {\ 492 if(a_fabs(value(r,c))>=a_prec) return 493 }\ 494 }\ 495 return true;\ 496 }\ 497 \ 498 template <class PREC>\ 499 bool is_decomplex(const PREC& a_prec,PREC(*a 500 /* Look if even dim matrix is of the form 501 502 unsigned int _D = dimension();\ 503 unsigned int _D_2 = _D/2;\ 504 if(2*_D_2!=_D) return false;\ 505 for(unsigned int r=0;r<_D_2;r++) {\ 506 for(unsigned int c=0;c<_D_2;c++) {\ 507 if(a_fabs(value(r,c)-value(r+_D_2,c+_D 508 }\ 509 }\ 510 for(unsigned int r=0;r<_D_2;r++) {\ 511 for(unsigned int c=_D_2;c<_D;c++) {\ 512 if(a_fabs(value(r,c)+value(r+_D_2,c-_D 513 }\ 514 }\ 515 return true;\ 516 }\ 517 \ 518 template <class PREC>\ 519 T determinant_prec(unsigned int a_tmp_rs[],u 520 const PREC& a_prec,PREC(* 521 unsigned int ord = dimension();\ 522 if(ord==0) {\ 523 return zero();\ 524 } else if(ord==1) {\ 525 return *m_vec;\ 526 } else if(ord==2) {\ 527 T v00 = *m_vec;\ 528 T v10 = *(m_vec+1);\ 529 T v01 = *(m_vec+2);\ 530 T v11 = *(m_vec+3);\ 531 return (v00 * v11 - v10 * v01);\ 532 } else if(ord==3) {\ 533 /* 00 01 02 \ 534 10 11 12 \ 535 20 21 22 \ 536 */\ 537 T v00 = *m_vec;\ 538 T v10 = *(m_vec+1);\ 539 T v20 = *(m_vec+2);\ 540 T v01 = *(m_vec+3);\ 541 T v11 = *(m_vec+4);\ 542 T v21 = *(m_vec+5);\ 543 T v02 = *(m_vec+6);\ 544 T v12 = *(m_vec+7);\ 545 T v22 = *(m_vec+8);\ 546 T cof_00 = v11 * v22 - v21 * v12;\ 547 T cof_10 = v01 * v22 - v21 * v02;\ 548 T cof_20 = v01 * v12 - v11 * v02;\ 549 return (v00*cof_00-v10*cof_10+v20*cof_20 550 }\ 551 \ 552 unsigned int rord = ord-1;\ 553 \ 554 T v_rc;\ 555 \ 556 T det = zero();\ 557 {for(unsigned int i=0;i<rord;i++) {a_tmp_cs 558 unsigned int c = 0;\ 559 \ 560 {for(unsigned int i=0;i<rord;i++) {a_tmp_rs 561 bool sg = true; /*c=0+r=0*/\ 562 for(unsigned int r=0;r<ord;r++) {\ 563 if(r>=1) a_tmp_rs[r-1] = r-1;\ 564 v_rc = value(r,c);\ 565 if(!is_zero(v_rc,a_prec,a_fabs)) {\ 566 T subdet = sub_determinant_prec(rord,a 567 if(sg) \ 568 det += v_rc * subdet;\ 569 else\ 570 det -= v_rc * subdet;\ 571 }\ 572 sg = sg?false:true;\ 573 }\ 574 \ 575 return det;\ 576 }\ 577 \ 578 T determinant(unsigned int a_tmp_rs[],unsign 579 return determinant_prec<double>(a_tmp_rs,a 580 }\ 581 \ 582 T determinant() const {\ 583 unsigned int ord = dimension();\ 584 if(ord==0) {\ 585 return zero();\ 586 } else if(ord==1) {\ 587 return *m_vec;\ 588 } else if(ord==2) {\ 589 T v00 = *m_vec;\ 590 T v10 = *(m_vec+1);\ 591 T v01 = *(m_vec+2);\ 592 T v11 = *(m_vec+3);\ 593 return (v00 * v11 - v10 * v01);\ 594 } else if(ord==3) {\ 595 /* 00 01 02 \ 596 10 11 12 \ 597 20 21 22 \ 598 */\ 599 T v00 = *m_vec;\ 600 T v10 = *(m_vec+1);\ 601 T v20 = *(m_vec+2);\ 602 T v01 = *(m_vec+3);\ 603 T v11 = *(m_vec+4);\ 604 T v21 = *(m_vec+5);\ 605 T v02 = *(m_vec+6);\ 606 T v12 = *(m_vec+7);\ 607 T v22 = *(m_vec+8);\ 608 T cof_00 = v11 * v22 - v21 * v12;\ 609 T cof_10 = v01 * v22 - v21 * v02;\ 610 T cof_20 = v01 * v12 - v11 * v02;\ 611 return (v00*cof_00-v10*cof_10+v20*cof_20 612 }\ 613 unsigned int rord = ord-1;\ 614 unsigned int* rs = new unsigned int[rord]; 615 unsigned int* cs = new unsigned int[rord]; 616 T det = determinant(rs,cs);\ 617 delete [] rs;\ 618 delete [] cs;\ 619 return det;\ 620 }\ 621 \ 622 template <class PREC>\ 623 bool invert_prec(TOOLS_MAT_CLASS& a_res,\ 624 unsigned int a_tmp_rs[],uns 625 const PREC& a_prec,PREC(*a_ 626 ) const { \ 627 unsigned int ord = dimension();\ 628 if(ord==0) return true;\ 629 \ 630 if(ord==1) {\ 631 T det = value(0,0);\ 632 if(is_zero(det,a_prec,a_fabs)) return fa 633 a_res.set_value(0,0,one()/det);\ 634 return true;\ 635 } else if(ord==2) {\ 636 T v00 = *m_vec;\ 637 T v10 = *(m_vec+1);\ 638 T v01 = *(m_vec+2);\ 639 T v11 = *(m_vec+3);\ 640 T det = (v00 * v11 - v10 * v01);\ 641 if(is_zero(det,a_prec,a_fabs)) return fa 642 a_res.set_value(0,0,v11/det);\ 643 a_res.set_value(1,1,v00/det);\ 644 a_res.set_value(0,1,minus_one()*v01/det) 645 a_res.set_value(1,0,minus_one()*v10/det) 646 return true;\ 647 } else if(ord==3) {\ 648 /* 00 01 02 \ 649 10 11 12 \ 650 20 21 22 \ 651 */\ 652 T v00 = *m_vec;\ 653 T v10 = *(m_vec+1);\ 654 T v20 = *(m_vec+2);\ 655 T v01 = *(m_vec+3);\ 656 T v11 = *(m_vec+4);\ 657 T v21 = *(m_vec+5);\ 658 T v02 = *(m_vec+6);\ 659 T v12 = *(m_vec+7);\ 660 T v22 = *(m_vec+8);\ 661 T cof_00 = v11 * v22 - v21 * v12;\ 662 T cof_10 = v01 * v22 - v21 * v02;\ 663 T cof_20 = v01 * v12 - v11 * v02;\ 664 T det = (v00*cof_00-v10*cof_10+v20*cof_2 665 if(is_zero(det,a_prec,a_fabs)) return fa 666 T cof_01 = v10 * v22 - v20 * v12;\ 667 T cof_11 = v00 * v22 - v20 * v02;\ 668 T cof_21 = v00 * v12 - v10 * v02;\ 669 T cof_02 = v10 * v21 - v20 * v11;\ 670 T cof_12 = v00 * v21 - v20 * v01;\ 671 T cof_22 = v00 * v11 - v10 * v01;\ 672 a_res.set_value(0,0,cof_00/det);\ 673 a_res.set_value(1,0,minus_one()*cof_01/d 674 a_res.set_value(2,0,cof_02/det);\ 675 a_res.set_value(0,1,minus_one()*cof_10/d 676 a_res.set_value(1,1,cof_11/det);\ 677 a_res.set_value(2,1,minus_one()*cof_12/d 678 a_res.set_value(0,2,cof_20/det);\ 679 a_res.set_value(1,2,minus_one()*cof_21/d 680 a_res.set_value(2,2,cof_22/det);\ 681 return true;\ 682 }\ 683 \ 684 /*Generic invertion method.*/\ 685 \ 686 unsigned int rord = ord-1;\ 687 \ 688 /* Get det with r = 0;*/\ 689 T det = zero();\ 690 T subdet;\ 691 {\ 692 {for(unsigned int i=0;i<rord;i++) {a_tmp_rs 693 unsigned int r = 0;\ 694 /*if(r>=1) a_tmp_rs[r-1] = r-1;*/\ 695 \ 696 {for(unsigned int i=0;i<rord;i++) {a_tmp_cs 697 bool sg = true; /*r=0+c=0*/\ 698 for(unsigned int c=0;c<ord;c++) {\ 699 if(c>=1) a_tmp_cs[c-1] = c-1;\ 700 subdet = sub_determinant_prec(rord,a_tmp 701 if(sg) {\ 702 det += value(r,c) * subdet;\ 703 a_res.set_value(c,r,subdet);\ 704 sg = false;\ 705 } else {\ 706 det += value(r,c) * subdet * minus_one 707 a_res.set_value(c,r,subdet * minus_one 708 sg = true;\ 709 }\ 710 }}\ 711 \ 712 if(is_zero(det,a_prec,a_fabs)) return false 713 \ 714 {for(unsigned int c=0;c<ord;c++) {\ 715 a_res.set_value(c,0,a_res.value(c,0)/det 716 }}\ 717 \ 718 {for(unsigned int i=0;i<rord;i++) {a_tmp_rs 719 bool sgr = false; /*r=1+c=0*/\ 720 for(unsigned int r=1;r<ord;r++) {\ 721 if(r>=1) a_tmp_rs[r-1] = r-1;\ 722 {for(unsigned int i=0;i<rord;i++) {a_tmp_ 723 bool sg = sgr;\ 724 for(unsigned int c=0;c<ord;c++) {\ 725 if(c>=1) a_tmp_cs[c-1] = c-1;\ 726 subdet = sub_determinant_prec(rord,a_t 727 if(sg) {\ 728 a_res.set_value(c,r,subdet/det);\ 729 sg = false;\ 730 } else {\ 731 a_res.set_value(c,r,(subdet * minus_ 732 sg = true;\ 733 }\ 734 }\ 735 sgr = sgr?false:true;\ 736 }\ 737 \ 738 return true;\ 739 }\ 740 \ 741 template <class PREC>\ 742 bool invert_prec(TOOLS_MAT_CLASS& a_res,cons 743 unsigned int ord = dimension();\ 744 if(ord==0) return true;\ 745 \ 746 if(ord==1) {\ 747 T det = value(0,0);\ 748 if(is_zero(det,a_prec,a_fabs)) return fa 749 a_res.set_value(0,0,one()/det);\ 750 return true;\ 751 } else if(ord==2) {\ 752 T v00 = *m_vec;\ 753 T v10 = *(m_vec+1);\ 754 T v01 = *(m_vec+2);\ 755 T v11 = *(m_vec+3);\ 756 T det = (v00 * v11 - v10 * v01);\ 757 if(is_zero(det,a_prec,a_fabs)) return fa 758 a_res.set_value(0,0,v11/det);\ 759 a_res.set_value(1,1,v00/det);\ 760 a_res.set_value(0,1,minus_one()*v01/det) 761 a_res.set_value(1,0,minus_one()*v10/det) 762 return true;\ 763 } else if(ord==3) {\ 764 /* 00 01 02 \ 765 10 11 12 \ 766 20 21 22 \ 767 */\ 768 T v00 = *m_vec;\ 769 T v10 = *(m_vec+1);\ 770 T v20 = *(m_vec+2);\ 771 T v01 = *(m_vec+3);\ 772 T v11 = *(m_vec+4);\ 773 T v21 = *(m_vec+5);\ 774 T v02 = *(m_vec+6);\ 775 T v12 = *(m_vec+7);\ 776 T v22 = *(m_vec+8);\ 777 T cof_00 = v11 * v22 - v21 * v12;\ 778 T cof_10 = v01 * v22 - v21 * v02;\ 779 T cof_20 = v01 * v12 - v11 * v02;\ 780 T det = (v00*cof_00-v10*cof_10+v20*cof_2 781 if(is_zero(det,a_prec,a_fabs)) return fa 782 T cof_01 = v10 * v22 - v20 * v12;\ 783 T cof_11 = v00 * v22 - v20 * v02;\ 784 T cof_21 = v00 * v12 - v10 * v02;\ 785 T cof_02 = v10 * v21 - v20 * v11;\ 786 T cof_12 = v00 * v21 - v20 * v01;\ 787 T cof_22 = v00 * v11 - v10 * v01;\ 788 a_res.set_value(0,0,cof_00/det);\ 789 a_res.set_value(1,0,minus_one()*cof_01/d 790 a_res.set_value(2,0,cof_02/det);\ 791 a_res.set_value(0,1,minus_one()*cof_10/d 792 a_res.set_value(1,1,cof_11/det);\ 793 a_res.set_value(2,1,minus_one()*cof_12/d 794 a_res.set_value(0,2,cof_20/det);\ 795 a_res.set_value(1,2,minus_one()*cof_21/d 796 a_res.set_value(2,2,cof_22/det);\ 797 return true;\ 798 }\ 799 \ 800 unsigned int rord = ord-1;\ 801 unsigned int* cs = new unsigned int[rord]; 802 unsigned int* rs = new unsigned int[rord]; 803 bool status = invert_prec(a_res,rs,cs,a_pr 804 delete [] cs;\ 805 delete [] rs;\ 806 return status;\ 807 }\ 808 \ 809 bool invert(TOOLS_MAT_CLASS& a_res,unsigned 810 return invert_prec<double>(a_res,a_tmp_rs, 811 }\ 812 \ 813 bool invert(TOOLS_MAT_CLASS& a_res) const {\ 814 return invert_prec<double>(a_res,0,zero_fa 815 }\ 816 \ 817 void power(unsigned int a_n,TOOLS_MAT_CLASS& 818 a_res.set_identity();\ 819 T* _tmp = new T[dim2()];\ 820 for(unsigned int i=0;i<a_n;i++) {\ 821 a_res._mul_mtx(m_vec,_tmp);\ 822 }\ 823 delete [] _tmp;\ 824 }\ 825 \ 826 void exp(unsigned int a_le,TOOLS_MAT_CLASS& 827 /* result = I + M + M**2/2! + M**3/3! + .. 828 a_res.set_identity();\ 829 a_tmp.set_identity();\ 830 for(unsigned int i=1;i<=a_le;i++) {\ 831 a_tmp._mul_mtx(m_vec,a_tmp_2);\ 832 a_tmp.multiply(one()/T(i));\ 833 a_res += a_tmp;\ 834 }\ 835 }\ 836 \ 837 void exp(unsigned int a_le,TOOLS_MAT_CLASS& 838 /* result = I + M + M**2/2! + M**3/3! + .. 839 TOOLS_MAT_CLASS tmp(*this);\ 840 tmp.set_identity();\ 841 T* tmp_2 = new T[dim2()];\ 842 exp(a_le,a_res,tmp,tmp_2);\ 843 delete [] tmp_2;\ 844 }\ 845 \ 846 void cosh(unsigned int a_le,TOOLS_MAT_CLASS& 847 /* result = I + M**2/2! + M**4/4! + .... * 848 a_res.set_identity();\ 849 TOOLS_MAT_CLASS M_2(*this);\ 850 M_2._mul_mtx(m_vec);\ 851 TOOLS_MAT_CLASS tmp(*this);\ 852 tmp.set_identity();\ 853 T* _tmp = new T[dim2()];\ 854 for(unsigned int i=1;i<=a_le;i+=2) {\ 855 tmp._mul_mtx(M_2.m_vec,_tmp);\ 856 tmp.multiply(one()/T(i+0)); \ 857 tmp.multiply(one()/T(i+1)); \ 858 a_res += tmp;\ 859 }\ 860 delete [] _tmp;\ 861 }\ 862 \ 863 void sinh(unsigned int a_le,TOOLS_MAT_CLASS& 864 /* result = M + M**3/3! + .... */\ 865 a_res._copy(m_vec);\ 866 TOOLS_MAT_CLASS M_2(*this);\ 867 M_2._mul_mtx(m_vec);\ 868 TOOLS_MAT_CLASS tmp(*this);\ 869 tmp._copy(m_vec);\ 870 T* _tmp = new T[dim2()];\ 871 for(unsigned int i=1;i<=a_le;i+=2) {\ 872 tmp._mul_mtx(M_2.m_vec,_tmp);\ 873 tmp.multiply(one()/T(i+1)); \ 874 tmp.multiply(one()/T(i+2)); \ 875 a_res += tmp;\ 876 }\ 877 delete [] _tmp;\ 878 }\ 879 \ 880 void log(unsigned int a_le,TOOLS_MAT_CLASS& 881 /* result = (M-I) - (M-I)**2/2 + (M-I)**3/ 882 /* WARNING : touchy, it may not converge ! 883 a_res.set_zero();\ 884 \ 885 TOOLS_MAT_CLASS M_I;\ 886 M_I.set_identity();\ 887 M_I.multiply(minus_one());\ 888 M_I._add_mtx(m_vec);\ 889 \ 890 TOOLS_MAT_CLASS M_Ip(M_I);\ 891 T fact = minus_one();\ 892 \ 893 TOOLS_MAT_CLASS tmp;\ 894 \ 895 T* _tmp = new T[dim2()];\ 896 for(unsigned int i=0;i<=a_le;i++) {\ 897 fact *= minus_one(); \ 898 tmp = M_Ip;\ 899 tmp.multiply(fact/T(i+1)); \ 900 a_res += tmp;\ 901 M_Ip._mul_mtx(M_I.m_vec,_tmp);\ 902 }\ 903 delete [] _tmp;\ 904 }\ 905 \ 906 void omega(unsigned int a_le,TOOLS_MAT_CLASS 907 /* result = I + M/2! + M**2/3! + M**3/4! + 908 a_res.set_identity();\ 909 a_tmp.set_identity();\ 910 for(unsigned int i=1;i<=a_le;i++) {\ 911 a_tmp._mul_mtx(m_vec,a_tmp_2);\ 912 a_tmp.multiply(one()/T(i+1));\ 913 a_res += a_tmp;\ 914 }\ 915 }\ 916 \ 917 void omega(unsigned int a_le,TOOLS_MAT_CLASS 918 /* result = I + M/2! + M**2/3! + M**3/4! + 919 TOOLS_MAT_CLASS tmp(*this);\ 920 tmp.set_identity();\ 921 T* tmp_2 = new T[dim2()];\ 922 omega(a_le,a_res,tmp,tmp_2);\ 923 delete [] tmp_2;\ 924 }\ 925 \ 926 template <class MAT>\ 927 bool copy(const MAT& a_from) {\ 928 /*for exa from a double matrix to a symbol 929 unsigned int _D = dimension();\ 930 if(a_from.dimension()!=_D) return false;\ 931 for(unsigned int r=0;r<_D;r++) {\ 932 for(unsigned int c=0;c<_D;c++) {\ 933 set_value(r,c,a_from.value(r,c));\ 934 }\ 935 }\ 936 return true;\ 937 }\ 938 public: /*operators*/\ 939 T operator()(unsigned int a_r,unsigned int a 940 /*WARNING : no check on a_r,a_c.*/\ 941 return m_vec[a_r + a_c * dimension()];\ 942 }\ 943 \ 944 T& operator[](size_t a_index) { /*for tools/ 945 /*WARNING : no check on a_index.*/\ 946 return m_vec[a_index];\ 947 }\ 948 const T& operator[](size_t a_index) const {\ 949 /*WARNING : no check on a_index.*/\ 950 return m_vec[a_index];\ 951 }\ 952 bool operator==(const TOOLS_MAT_CLASS& a_arr 953 return equal(a_array);\ 954 }\ 955 bool operator!=(const TOOLS_MAT_CLASS& a_arr 956 return !operator==(a_array);\ 957 }\ 958 TOOLS_MAT_CLASS& operator*=(const TOOLS_MAT_ 959 _mul_mtx(a_m.m_vec);\ 960 return *this;\ 961 }\ 962 TOOLS_MAT_CLASS& operator+=(const TOOLS_MAT_ 963 _add_mtx(a_m.m_vec);\ 964 return *this;\ 965 }\ 966 TOOLS_MAT_CLASS& operator-=(const TOOLS_MAT_ 967 _sub_mtx(a_m.m_vec);\ 968 return *this;\ 969 }\ 970 TOOLS_MAT_CLASS& operator*=(const T& a_fac) 971 for(unsigned int i=0;i<dim2();i++) m_vec[i 972 return *this;\ 973 }\ 974 \ 975 TOOLS_MAT_CLASS operator*(const T& a_fac) {\ 976 TOOLS_MAT_CLASS res;\ 977 res.operator*=(a_fac);\ 978 return res;\ 979 }\ 980 protected:\ 981 void _copy(const T a_m[]) {\ 982 T* tp = (T*)m_vec;T* ap = (T*)a_m;\ 983 for(unsigned int i=0;i<dim2();i++,tp++,ap+ 984 /*{for(unsigned int i=0;i<dim2();i++) m_vec 985 /* memcpy does not work with std::complex<> 986 /*vec_copy(m_vec,a_m,dim2());*/\ 987 }\ 988 \ 989 void _add_mtx(const T a_m[]) { /* this = th 990 T* tp = (T*)m_vec;T* ap = (T*)a_m;\ 991 for(unsigned int i=0;i<dim2();i++,tp++,ap+ 992 /*for(unsigned int i=0;i<dim2();i++) m_vec 993 /*vec_add(m_vec,a_m,m_vec,dim2());*/\ 994 }\ 995 void _sub_mtx(const T a_m[]) { /* this = th 996 T* tp = (T*)m_vec;T* ap = (T*)a_m;\ 997 for(unsigned int i=0;i<dim2();i++,tp++,ap+ 998 /*for(unsigned int i=0;i<dim2();i++) m_vec 999 /*vec_sub(m_vec,a_m,m_vec,dim2());*/\ 1000 }\ 1001 \ 1002 /*\ 1003 void _mul_mtx(const T a_m[],T a_tmp[]) {\ 1004 unsigned int ord = dimension();\ 1005 for(unsigned int r=0;r<ord;r++) {\ 1006 for(unsigned int c=0;c<ord;c++) {\ 1007 T _value = zero();\ 1008 for(unsigned int i=0;i<ord;i++) {\ 1009 _value += (*(m_vec+r+i*ord)) * (*(a 1010 }\ 1011 *(a_tmp+r+c*ord) = _value;\ 1012 }\ 1013 }\ 1014 _copy(a_tmp);\ 1015 }\ 1016 */\ 1017 void _mul_mtx(const T a_m[],T a_tmp[]) { /* 1018 /* this = this * a_m */\ 1019 typedef T* Tp;\ 1020 Tp tpos,ttpos,rpos,apos,mpos,aapos;\ 1021 T _value;\ 1022 unsigned int r,c,i;\ 1023 \ 1024 unsigned int _D = dimension();\ 1025 \ 1026 tpos = a_tmp;\ 1027 for(r=0;r<_D;r++,tpos++) {\ 1028 ttpos = tpos;\ 1029 rpos = m_vec+r;\ 1030 apos = (T*)a_m;\ 1031 for(c=0;c<_D;c++,ttpos+=_D,apos+=_D) {\ 1032 _value = zero();\ 1033 mpos = rpos;\ 1034 aapos = apos;\ 1035 for(i=0;i<_D;i++,mpos+=_D,aapos++) _v 1036 *ttpos = _value;\ 1037 }\ 1038 }\ 1039 _copy(a_tmp);\ 1040 }\ 1041 \ 1042 void _mul_mtx(const T a_m[]) {\ 1043 T* _tmp = new T[dim2()];\ 1044 _mul_mtx(a_m,_tmp);\ 1045 delete [] _tmp;\ 1046 }\ 1047 \ 1048 void _left_mul_mtx(const T a_m[]) {\ 1049 /* this = a_m * this */\ 1050 unsigned int _D = dimension();\ 1051 T* _tmp = new T[dim2()];\ 1052 for(unsigned int r=0;r<_D;r++) {\ 1053 for(unsigned int c=0;c<_D;c++) {\ 1054 T _value = zero();\ 1055 for(unsigned int i=0;i<_D;i++) {\ 1056 _value += (*(a_m+r+i*_D)) * (*(m_ve 1057 }\ 1058 *(_tmp+r+c*_D) = _value;\ 1059 }\ 1060 }\ 1061 _copy(_tmp);\ 1062 delete [] _tmp;\ 1063 }\ 1064 \ 1065 template <class PREC>\ 1066 T sub_determinant_prec(unsigned int a_ord,u 1067 const PREC& a_prec,P 1068 /*WARNING : to optimize, we do not check 1069 unsigned int ord = a_ord;\ 1070 if(ord==0) return zero();\ 1071 else if(ord==1) return value(aRs[0],aCs[0 1072 else if(ord==2) {\ 1073 /*return (value(aRs[0],aCs[0]) * value( 1074 value(aRs[1],aCs[0]) * value(a 1075 Optimize the upper :*/\ 1076 \ 1077 unsigned int _ord = dimension();\ 1078 \ 1079 return ( (*(m_vec+aRs[0]+aCs[0]*_ord)) 1080 (*(m_vec+aRs[1]+aCs[0]*_ord)) 1081 \ 1082 } else if(ord==3) {\ 1083 /* 00 01 02 \ 1084 10 11 12 \ 1085 20 21 22 \ 1086 */\ 1087 unsigned int _ord = dimension();\ 1088 \ 1089 T v00 = *(m_vec+aRs[0]+aCs[0]*_ord);\ 1090 T v10 = *(m_vec+aRs[1]+aCs[0]*_ord);\ 1091 T v20 = *(m_vec+aRs[2]+aCs[0]*_ord);\ 1092 T v01 = *(m_vec+aRs[0]+aCs[1]*_ord);\ 1093 T v11 = *(m_vec+aRs[1]+aCs[1]*_ord);\ 1094 T v21 = *(m_vec+aRs[2]+aCs[1]*_ord);\ 1095 T v02 = *(m_vec+aRs[0]+aCs[2]*_ord);\ 1096 T v12 = *(m_vec+aRs[1]+aCs[2]*_ord);\ 1097 T v22 = *(m_vec+aRs[2]+aCs[2]*_ord);\ 1098 T cof_00 = v11 * v22 - v21 * v12;\ 1099 T cof_10 = v01 * v22 - v21 * v02;\ 1100 T cof_20 = v01 * v12 - v11 * v02;\ 1101 return (v00*cof_00-v10*cof_10+v20*cof_2 1102 }\ 1103 \ 1104 unsigned int rord = ord-1;\ 1105 unsigned int* cs = new unsigned int[rord] 1106 unsigned int* rs = new unsigned int[rord] 1107 \ 1108 T v_rc;\ 1109 \ 1110 T det = zero();\ 1111 {for(unsigned int i=0;i<rord;i++) {cs[i] = 1112 unsigned int c = 0;\ 1113 /*if(c>=1) cs[c-1] = c-1;*/\ 1114 \ 1115 {for(unsigned int i=0;i<rord;i++) {rs[i] = 1116 bool sg = true; /*c=0+r=0*/\ 1117 for(unsigned int r=0;r<ord;r++) {\ 1118 if(r>=1) rs[r-1] = aRs[r-1];\ 1119 v_rc = value(aRs[r],aCs[c]);\ 1120 if(!is_zero(v_rc,a_prec,a_fabs)) {\ 1121 T subdet = sub_determinant_prec(rord, 1122 if(sg)\ 1123 det += v_rc * subdet;\ 1124 else\ 1125 det -= v_rc * subdet;\ 1126 }\ 1127 sg = sg?false:true;\ 1128 }\ 1129 \ 1130 delete [] cs;\ 1131 delete [] rs;\ 1132 \ 1133 return det;\ 1134 }\ 1135 \ 1136 static double zero_fabs(const T& a_number) 1137 1138 #endif