12 #include "CLHEP/Matrix/Matrix.h"
13 #include "CLHEP/Matrix/Vector.h"
14 #include "CLHEP/Matrix/SymMatrix.h"
18 static inline int sign(
double x) {
return (
x>0 ? 1: -1);}
62 (*b)(
b->num_row()) /=
R(
b->num_row(),
b->num_row());
64 int nb =
b->num_row();
67 for (
int r=
b->num_row()-1;r>=1;--r) {
70 for (
int c=r+1;c<=
b->num_row();c++) {
71 (*br)-=(*(Rrc++))*(*(bc++));
90 int nb =
b->num_row();
91 int nc =
b->num_col();
93 for (
int i=1;
i<=
b->num_col();
i++) {
94 (*b)(
b->num_row(),
i) /=
R(
b->num_row(),
b->num_row());
97 for (
int r=
b->num_row()-1;r>=1;--r) {
100 for (
int c=r+1;c<=
b->num_row();c++) {
101 (*bri)-= (*(Rrc++)) * (*bci);
102 if(c<b->num_row()) bci += nc;
122 int k1,
int k2,
int row_min,
int row_max) {
123 if (row_max<=0) row_max = A->
num_row();
127 for (
int j=row_min;
j<=row_max;
j++) {
128 double tau1=(*Ajk1);
double tau2=(*Ajk2);
129 (*Ajk1)=c*tau1-ds*tau2;(*Ajk2)=ds*tau1+c*tau2;
152 int row,
int col,
int row_start,
int col_start) {
153 double beta=-2/vnormsq;
160 int n =
a->num_col();
161 int nv =
v.num_col();
164 for (c=col;c<=
a->num_col();c++) {
167 for (
int r=row;r<=
a->num_row();r++) {
168 (*wptr)+=(*(acr++))*(*vp);
172 if(c<a->num_col()) acrb +=
n;
180 for (
int r=row; r<=
a->num_row();r++) {
183 for (c=col;c<=
a->num_col();c++) {
184 (*(arc++))+=(*vp)*(*wptr);
188 if(r<a->num_row()) arcb +=
n;
203 max=min=fabs(mcopy(1,1));
207 for (
int i=2;
i<=
n;
i++) {
208 if (max<fabs(*mii)) max=fabs(*mii);
209 if (min>fabs(*mii)) min=fabs(*mii);
226 double d=(t->
fast(end-1,end-1)-t->
fast(end,end))/2;
228 (d+sign(d)*sqrt(d*d+ t->
fast(end,end-1)*t->
fast(end,end-1)));
229 double x=t->
fast(begin,begin)-
mu;
230 double z=t->
fast(begin+1,begin);
234 for (
int k=begin;
k<=end-1;
k++) {
244 *(tkk-1)= *(tkk-1)*c-(*(tkp1k-1))*ds;
249 double aq=(*tkp1k+1);
250 (*tkk)=ap*c*c-2*c*bp*ds+aq*ds*ds;
251 (*tkp1k)=c*ap*ds+bp*c*c-bp*ds*ds-ds*aq*c;
252 (*(tkp1k+1))=ap*ds*ds+2*c*bp*ds+aq*c*c;
255 double bq=(*(tkp2k+1));
263 if(
k<end-2) tkp2k +=
k+3;
269 double d=(t->
fast(end-1,end-1)-t->
fast(end,end))/2;
271 (d+sign(d)*sqrt(d*d+ t->
fast(end,end-1)*t->
fast(end,end-1)));
272 double x=t->
fast(begin,begin)-
mu;
273 double z=t->
fast(begin+1,begin);
277 for (
int k=begin;
k<=end-1;
k++) {
287 *(tkk-1)= (*(tkk-1))*c-(*(tkp1k-1))*ds;
292 double aq=(*(tkp1k+1));
293 (*tkk)=ap*c*c-2*c*bp*ds+aq*ds*ds;
294 (*tkp1k)=c*ap*ds+bp*c*c-bp*ds*ds-ds*aq*c;
295 (*(tkp1k+1))=ap*ds*ds+2*c*bp*ds+aq*c*c;
297 double bq=(*(tkp2k+1));
305 if(
k<end-2) tkp2k +=
k+3;
317 const double tolerance = 1e-12;
325 for (
int i=begin;
i<=end-1;
i++) {
327 tolerance*(fabs(*sii)+fabs(*(sip1i+1)))) {
335 while(begin<end && hms->fast(begin+1,begin) ==0) begin++;
336 while(end>begin && hms->
fast(end,end-1) ==0) end--;
357 for (
i=row;
i<=col;
i++) {
358 (*(vp++))=(*(aci++));
360 for (;
i<=
a.num_row();
i++) {
364 v(1)+=sign(
a(row,col))*
v.norm();
375 for (
int i=row;
i<=
a.num_row();
i++) {
379 v(1)+=sign(
a(row,col))*
v.norm();
398 int n =
a->num_col();
401 for (r=row;r<=
a->num_row();r++) {
403 if(r<a->num_row()) arc +=
n;
405 double normsq=
v.normsq();
406 double norm=sqrt(normsq);
408 v(1)+=sign((*
a)(row,col))*
norm;
410 (*a)(row,col)=-sign((*
a)(row,col))*
norm;
411 if (row<a->num_row()) {
412 arc =
a->m.begin() + row *
n + (col-1);
413 for (r=row+1;r<=
a->num_row();r++) {
415 if(r<a->num_row()) arc +=
n;
424 int nv =
v->num_col();
425 int na =
a->num_col();
429 for (r=row;r<=
a->num_row();r++) {
431 normsq+=(*vrc)*(*vrc);
437 double norm=sqrt(normsq);
438 vrc =
v->m.begin() + (row-1) * nv + (col-1);
439 normsq-=(*vrc)*(*vrc);
440 (*vrc)+=sign((*
a)(row,col))*
norm;
441 normsq+=(*vrc)*(*vrc);
442 (*a)(row,col)=-sign((*
a)(row,col))*
norm;
443 if (row<a->num_row()) {
444 arc =
a->m.begin() + row * na + (col-1);
445 for (r=row+1;r<=
a->num_row();r++) {
447 if(r<a->num_row()) arc += na;
462 int nv =
v->num_col();
466 for (r=row;r<=
a->num_row();r++)
469 normsq+=(*vrc)*(*vrc);
475 double norm=sqrt(normsq);
476 vrc =
v->m.begin() + (row-1) * nv + (col-1);
477 arc =
a->m.begin() + (row-1) * row / 2 + (col-1);
478 (*vrc)+=sign(*arc)*
norm;
479 (*arc)=-sign(*arc)*
norm;
481 for (r=row+1;r<=
a->num_row();r++) {
483 if(r<a->num_row()) arc += r;
532 const double small = 1e-10;
536 for (
int i=0;
i<
n;
i++)
538 if (fabs(t=A[
i].normsq())<
small) {
585 int k1,
int k2,
int col_min,
int col_max) {
587 if (col_max==0) col_max = A->
num_col();
591 for (
int j=col_min;
j<=col_max;
j++) {
592 double tau1=(*Ak1j);
double tau2=(*Ak2j);
593 (*(Ak1j++))=c*tau1-ds*tau2;(*(Ak2j++))=ds*tau1+c*tau2;
612 double beta=-2/vnormsq;
618 int na =
a->num_col();
622 for (c=col;c<=
a->num_col();c++) {
625 for (
int r=row;r<=
a->num_row();r++) {
626 (*wptr)+=(*arc)*(*(vp++));
627 if(r<a->num_row()) arc += na;
636 arcb =
a->m.begin() + (row-1) * na + (col-1);
638 for (
int r=row; r<=
a->num_row();r++) {
641 for (c=col;c<=
a->num_col();c++) {
642 (*(arc++))+=(*vp)*(*(wptr2++));
645 if(r<a->num_row()) arcb += na;
650 int row,
int col,
int row_start,
int col_start) {
651 double beta=-2/vnormsq;
655 int na =
a->num_col();
656 int nv =
v.num_col();
661 for (c=col;c<=
a->num_col();c++) {
664 for (
int r=row;r<=
a->num_row();r++) {
665 (*wptr)+=(*arc)*(*vpc);
676 arcb =
a->m.begin() + (row-1) * na + (col-1);
678 for (
int r=row; r<=
a->num_row();r++) {
681 for (c=col;c<=
a->num_col();c++) {
682 (*(arc++))+=(*vpc)*(*(wptr2++));
715 for (
int r=1;r<=b2.
num_row();r++) {
718 for (
int c=1;c<=
b.num_row();c++) {
719 *b2r += (*Qcr) * (*(bc++));
720 if(c<
b.num_row()) Qcr +=
n;
740 int nb =
b.num_col();
744 for (
int i=1;
i<=
b.num_col();
i++) {
747 for (
int r=1;r<=b2.
num_row();r++) {
750 for (
int c=1;c<=
b.num_row();c++) {
751 *b2ri += (*Qcr) * (*bci);
777 for (
int k=1;
k<=
a->num_col()-2;
k++) {
785 for (
j=
k+2;
j<=
a->num_row();
j++) {
787 if(j<a->num_row()) ajk +=
j;
793 if(j<hsm->num_row()) hsmjkp += nh;
800 for (rptr=
k+1;rptr<=hsm->
num_row();rptr++) {
801 normsq+=(*hsmrptrkp)*(*hsmrptrkp);
802 if(rptr<hsm->num_row()) hsmrptrkp += nh;
811 for (cptr=
k+1;cptr<=rptr;cptr++) {
812 (*pr)+=
a->fast(rptr,cptr)*(*hsmcptrkp);
813 if(cptr<a->num_col()) hsmcptrkp += nh;
815 for (;cptr<=
a->num_col();cptr++) {
816 (*pr)+=
a->fast(cptr,rptr)*(*hsmcptrkp);
817 if(cptr<a->num_col()) hsmcptrkp += nh;
825 hsmrptrkp = hsm->m.begin() +
k * (nh+1) - 1;
827 pdotv+=(*(pr++))*(*hsmrptrkp);
828 if(r<p.
num_row()) hsmrptrkp += nh;
831 hsmrptrkp = hsm->m.begin() +
k * (nh+1) - 1;
833 (*(pr++))-=pdotv*(*hsmrptrkp)/normsq;
834 if(r<p.
num_row()) hsmrptrkp += nh;
838 hsmrptrkp = hsm->m.begin() +
k * (nh+1) - 1;
843 for (
int c=1;c<=r;c++) {
844 a->fast(rptr,cptr)-= (*hsmrptrkp)*(*(mpc++))+(*pr)*(*hsmcptrkp);
846 if(c<r) hsmcptrkp += nh;
850 if(r<p.
num_row()) hsmrptrkp += nh;
871 int row_start,
int col_start)
874 for (
int i=row_start;
i<=row_start+
a->num_row()-row;
i++)
875 normsq+=
v(
i,col)*
v(
i,col);
879 void givens(
double a,
double b,
double *c,
double *ds)
881 if (
b ==0) { *c=1; *ds=0; }
883 if (fabs(
b)>fabs(
a)) {
885 *ds=1/sqrt(1+tau*tau);
889 *c=1/sqrt(1+tau*tau);
902 int row_start,
int col_start)
905 int end = row_start+
a->num_row()-row;
906 for (
int i=row_start;
i<=end;
i++)
907 normsq +=
v(
i,col)*
v(
i,col);