12#include "CLHEP/Matrix/Matrix.h"
13#include "CLHEP/Matrix/Vector.h"
14#include "CLHEP/Matrix/SymMatrix.h"
18static 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();
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;
227 double mu=t->
fast(end,end)-t->
fast(end,end-1)*t->
fast(end,end-1)/
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;
270 double mu=t->
fast(end,end)-t->
fast(end,end-1)*t->
fast(end,end-1)/
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;
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;
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;
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) {
543 D +=
dot(A[i],
B[i])*(1-2/t)*A[i]+
B[i];
568 for (
int j=hsm.
num_col();j>=1;--j)
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();
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;
791 for (j=k+1;j<=hsm->
num_row();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;
863 for (
int j=hsm.
num_col();j>=1;--j) {
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);
876 col_house(
a,v,normsq,row,col,row_start,col_start);
879void 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);
897 for (
int i=1;i<=A->
num_col();i++)
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);
910 row_house(
a,v,normsq,row,col,row_start,col_start);
std::vector< double, Alloc< double, 25 > >::const_iterator mcIter
std::vector< double, Alloc< double, 25 > >::iterator mIter
static void error(const char *s)
virtual int num_col() const
virtual int num_row() const
const double & fast(int row, int col) const
virtual int num_row() const
void col_house(HepMatrix *a, const HepMatrix &v, double vnormsq, int row, int col, int row_start, int col_start)
void tridiagonal(HepSymMatrix *a, HepMatrix *hsm)
HepVector qr_solve(const HepMatrix &A, const HepVector &b)
HepSymMatrix vT_times_v(const HepVector &v)
void house_with_update2(HepSymMatrix *a, HepMatrix *v, int row=1, int col=1)
double norm(const HepGenMatrix &m)
HepMatrix qr_inverse(const HepMatrix &A)
void house_with_update(HepMatrix *a, int row=1, int col=1)
void back_solve(const HepMatrix &R, HepVector *b)
void qr_decomp(HepMatrix *A, HepMatrix *hsm)
double dot(const HepVector &v1, const HepVector &v2)
HepVector house(const HepMatrix &a, int row=1, int col=1)
HepVector min_line_dist(const HepVector *const A, const HepVector *const B, int n)
void row_givens(HepMatrix *A, double c, double s, int k1, int k2, int col_min=1, int col_max=0)
void givens(double a, double b, double *c, double *s)
void col_givens(HepMatrix *A, double c, double s, int k1, int k2, int row_min=1, int row_max=0)
void row_house(HepMatrix *a, const HepVector &v, double vnormsq, int row=1, int col=1)
double condition(const HepSymMatrix &m)
HepMatrix diagonalize(HepSymMatrix *s)
void diag_step(HepSymMatrix *t, int begin, int end)