StRoot  1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
xTCL.cxx
1 #include "xTCL.h"
2 #include "TArrayI.h"
3 #include <cassert>
4 
5 //______________________________________________________________________________
6 //______________________________________________________________________________
7 //______________________________________________________________________________
8 //______________________________________________________________________________
9 double xTCL::vmaxa(const double *a,int na)
10 {
11  double r=0;
12  for (int i=0;i<na;i++){if (r < TMath::Abs(a[i])) r = TMath::Abs(a[i]);}
13  return r;
14 }
15 //______________________________________________________________________________
16 double xTCL::vmaxa(const TVectorD &a)
17 {
18  return vmaxa(a.GetMatrixArray(),a.GetNrows());
19 }
20 //______________________________________________________________________________
21 int xTCL::lvmaxa(const double *a,int na)
22 {
23  double r=0;
24  int l=0;
25  for (int i=0;i<na;i++){if (r < TMath::Abs(a[i])) {r = TMath::Abs(a[i]);l=i;}}
26  return l;
27 }
28 //______________________________________________________________________________
29 int xTCL::lvmina(const double *a,int na)
30 {
31  double r=TMath::Abs(a[0]);
32  int l=0;
33  for (int i=1;i<na;i++){if (r > TMath::Abs(a[i])) {r = TMath::Abs(a[i]);l=i;}}
34  return l;
35 }
36 //______________________________________________________________________________
37 void xTCL::vfill(double *a,double f,int na)
38 {
39  for (int i=0;i<na;i++) {a[i]=f;}
40 }
41 //______________________________________________________________________________
42 /*
43 * $Id: xTCL.cxx,v 1.6 2020/04/05 18:55:08 perev Exp $
44 *
45 * $Log: xTCL.cxx,v $
46 * Revision 1.6 2020/04/05 18:55:08 perev
47 * Supress eigen2
48 *
49 * Revision 1.5 2014/02/18 19:45:49 perev
50 * Eigen2 copy of THelix one
51 *
52 * Revision 1.4 2009/08/28 16:38:55 fine
53 * fix the compilation issues under SL5_64_bits gcc 4.3.2
54 *
55 * Revision 1.3 2008/10/29 19:38:06 perev
56 * method toEuler added
57 *
58 * Revision 1.2 2007/07/12 20:38:41 fisyak
59 * Add includes for ROOT 5.16
60 *
61 * Revision 1.1 2007/04/26 04:22:31 perev
62 * eXtended TCL class xTCL
63 *
64 * Revision 1.1.1.1 1996/04/01 15:02:13 mclareni
65 * Mathlib gen
66 */
67 //_____________________________________________________________________________
68 #if 0
69 static void eigen2(double G[3], double lam[2], double eig[2])
70 {
71  double spur = G[0]+G[2];
72 //double det = G[0]*G[2]-G[1]*G[1];
73  double dis = (G[0]-G[2])*(G[0]-G[2])+4*G[1]*G[1];
74  dis = sqrt(dis);
75  if (lam) {
76  lam[0] = 0.5*(spur+dis);
77  lam[1] = 0.5*(spur-dis);
78  }
79  if (!eig) return;
80  double g[3]={G[0]-G[2]-dis,2*G[1],G[2]-G[0]-dis};
81  int kase =0;
82  for (int i=1;i<3;i++) { if (fabs(g[i])>fabs(g[kase])) kase = i;}
83  switch(kase) {
84  case 0: eig[0] = g[1]/g[0]; eig[1]=-1; break;
85  case 1: eig[1] = g[0]/g[1]; eig[0]=-1; break;
86  case 2: eig[1] = g[1]/g[2]; eig[0]=-1; break;
87  }
88  double nor = sqrt(eig[0]*eig[0]+eig[1]*eig[1]);
89  if (nor>1e-11) {
90  int j=(fabs(eig[0])>fabs(eig[1]))? 0:1;
91  if(eig[j]<0) nor = -nor;
92  eig[0]/=nor;eig[1]/=nor;}
93  else {
94  eig[0]=1;eig[1]=0;
95  }
96 }
97 #endif
98 //_____________________________________________________________________________
99 double xTCL::simpson(const double *F,double A,double B,int NP)
100 {
101  int N2=NP-1;
102  assert(N2>0 && !(N2&1));
103  double S1=F[N2-1];
104  double S2=0;
105 
106  for (int N = 1;N<=N2-3;N+=2) {S1+=F[N];S2+=F[N+1];}
107  S1=S1+S1+S2;
108  double H=(F[0]+F[N2]+S1+S1)*(B-A)/(3*N2);
109  return H;
110 }
111 //_____________________________________________________________________________
112 double **xTCL::makeMatrixD(int m,int n)
113 {
114  int szp = (m*sizeof(double*)+sizeof(double))/sizeof(double);
115  int szd =m*n;
116  double *d = new double[szp+szd];
117  memset(d,0,(szp+szd)*sizeof(double));
118  double **p = (double**)d;
119  d+=szp;
120  for (int i=0;i<m;i++) { p[i]=d; d+=n;};
121  return p;
122 }
123 //______________________________________________________________________________
124 void xTCL::mxmlrt(const TMatrixD &A,const TMatrixD &B,TMatrixD &X)
125 {
126  int nRowA = A.GetNrows();
127  int nColA = A.GetNcols();
128  int nRowB = B.GetNrows();
129  int nColB = B.GetNcols(); if(nColB){}
130  assert(nColA ==nRowB);
131  X.ResizeTo(nRowA,nRowA);
132  TCL::mxmlrt(A.GetMatrixArray(),B.GetMatrixArray()
133  ,X.GetMatrixArray(),nRowA,nColA);
134 
135 }
136 //______________________________________________________________________________
137 void xTCL::mxmlrtS(const double *A,const double *B,double *X,int nra,int nca)
138 {
139  TCL::vzero(X,nra*nra);
140  for (int i=0,ii=0;i<nra;i++,ii+=nca) {
141  for (int j=0,jj=0;j<nca;j++,jj+=nca) {
142  if(!A[ii+j]) continue;
143  for (int k=0,kk=0;k<=i;k++,kk+=nca) {
144  double &Xik =X[i*nra+k];
145  for (int l=0;l<nca;l++) {
146  if(!A[kk+l]) continue;
147  Xik +=A[ii+j]*A[kk+l]*B[jj+l];
148  } } } }
149  for (int i=0;i<nra;i++){
150  for (int k=0;k<i ;k++){X[k*nra+i] = X[i*nra+k];}}
151 }
152 //______________________________________________________________________________
153 void xTCL::mxmlrtS(const TMatrixD &A,const TMatrixD &B,TMatrixD &X)
154 {
155  int nRowA = A.GetNrows();
156  int nColA = A.GetNcols();
157  int nRowB = B.GetNrows();
158  int nColB = B.GetNcols(); if(nColB){}
159  assert(nColA ==nRowB);
160  X.ResizeTo(nRowA,nRowA);
161  xTCL::mxmlrtS(A.GetMatrixArray(),B.GetMatrixArray()
162  ,X.GetMatrixArray(),nRowA,nColA);
163 
164 }
165 //______________________________________________________________________________
166 TMatrixD xTCL::T(const TMatrixD &mx)
167 {
168  TMatrixD tmp(mx);
169  return tmp.T();
170 }
171 //______________________________________________________________________________
172 double xTCL::vasum(const double *a, int na)
173 {
174  double sum = 0;
175  for (int i=0;i<na;i++) { sum += TMath::Abs(a[i]);}
176  return sum;
177 }
178 //______________________________________________________________________________
179 double xTCL::vasum(const TVectorD &a)
180 {
181  return vasum(a.GetMatrixArray(),a.GetNrows());
182 }
183 //______________________________________________________________________________
184 int xTCL::SqProgSimple( TVectorD &x
185  ,const TVectorD &g,const TMatrixD &G
186  ,const TVectorD &Min
187  ,const TVectorD &Max, int iAktp)
188 {
189 static const double kSMALL = 1e-9;
190 static int nCall=0; nCall++;
191  enum {kINIT=0,kADDCONS,kFREECONS};
192  int kase = kINIT;
193  int nPars = g.GetNrows();
194  TVectorD xx(x),gg(g),add(nPars);
195  TArrayI Side(nPars);
196  int nCons=0,addCons = -1,freCons=-1,freSide=0,addSide=0,con=0;
197  double maxGra=3e33;
198  int iAkt = iAktp;
199  while(1946) {
200 // Eliminate outdated constrains
201  freCons=-1; freSide=0;
202  if (nCons && kase==kFREECONS ) {
203  double tryGra=kSMALL; freCons=-1;
204  for (int ix=0;ix<nPars;ix++) {
205  if(!Side[ix]) continue;
206  double gra = gg[ix]*Side[ix];
207  if (gra< tryGra) continue;
208  if (gra>=maxGra) continue;
209  freCons=ix; tryGra=gra;
210  }
211  if (freCons>=0) {
212  maxGra = tryGra;
213  freSide = Side[freCons];
214  Side[freCons]=0;
215  nCons--;
216  }
217  }
218 
219  if(kase==kFREECONS && freCons<0) { break;} //DONE
220 
221 // Make new matrix, etc...
222  TMatrixD S(G);
223  TVectorD B(gg);
224  if (nCons) {
225  for (int ix=0;ix<nPars;ix++) {
226  if (Side[ix]==0) continue;
227  for (int jx=0;jx<nPars;jx++) {S[ix][jx]=0; S[jx][ix]=0;}
228  S[ix][ix]=1; B[ix]=0;
229  } }
230  if (iAkt==0 ) {
231 
232  double det=12345; S.Invert(&det);
233 // if (det<0) iAkt=1;
234 // else add = (-1.)*(S*B);
235  add = (-1.)*(S*B);
236  double along = B*add;
237  if (along>0) add*=(-1.);
238  }
239 
240  if (iAkt==1 ) {
241  double bb = (B*B);
242  double bSb = (B*(S*B));
243  double tau = -bb/(TMath::Abs(bSb)+3e-33);
244  add = tau*B;
245 
246  }
247  if(kase==kFREECONS && freSide) { //Free constrain case
248  if (add[freCons]*freSide > -kSMALL) {
249  Side[freCons]=freSide; nCons++; continue;}
250  }
251 
252 // Do we need new constrain?
253  double fak=1;
254  addCons = -1; addSide = 0;
255  con = 0;
256  for (int ix=0;ix<nPars;ix++) {
257  if (Side[ix]) {add[ix]=0; con = 100*con+ix+1;continue;}
258  double xi = xx[ix]+fak*add[ix];
259  if (xi < Min[ix]){fak = (Min[ix]-xx[ix])/add[ix]; addCons=ix;addSide=-1;}
260  if (xi > Max[ix]){fak = (Max[ix]-xx[ix])/add[ix]; addCons=ix;addSide= 1;}
261  assert(fak<=1. && fak>=0.);
262  }
263  add*=fak;
264  xx+= add;
265  gg += G*add;
266  maxGra=3e33;
267  kase = kFREECONS; if (!addSide) continue;
268  kase = kADDCONS;
269  xx[addCons] = (addSide<0)? Min[addCons]:Max[addCons];
270 // Add new constrain;
271  Side[addCons] = addSide ;nCons++;
272 
273  } //end of while(1)
274  x = xx;
275  return abs(con);
276 }
277 //______________________________________________________________________________
278 void xTCL::toEuler(const double TT[3][3],double PhiThePsi[6])
279 {
280 
281 // TT[0][0] = cPsi*cPhi - sPsi*cThe*sPhi
282 // TT[1][0] = -sPsi*cPhi - cPsi*cThe*sPhi
283 // TT[2][0] = sThe*sPhi
284 // TT[0][1] = cPsi*sPhi + sPsi*cThe*cPhi
285 // TT[1][1] = -sPsi*sPhi + cPsi*cThe*cPhi
286 // TT[2][1] = -sThe*cPhi
287 // TT[0][2] = sPsi*sThe
288 // TT[1][2] = cPsi*sThe
289 // TT[2][2] = cThe
290 
291  double cThe = TT[2][2]; if (cThe>1) cThe=1; if (cThe<-1) cThe=-1;
292  double sThe = (TT[0][2]*TT[0][2]+TT[1][2]*TT[1][2])
293  + (TT[2][0]*TT[2][0]+TT[2][1]*TT[2][1]);
294  sThe = sqrt(0.5*sThe);
295 
296  double N = 0.5*(cThe*cThe+sThe*sThe+1);
297  cThe/=N; sThe/=N;
298 
299  double sPsi = 0,cPsi=1;
300  if (sThe>1e-6) {
301  sPsi = TT[0][2]/sThe; cPsi = TT[1][2]/sThe;
302  N = 0.5*(cPsi*cPsi+sPsi*sPsi+1);
303  cPsi/=N; sPsi/=N;
304  }
305  double cPhi = cPsi*TT[0][0]-sPsi*TT[1][0];
306  double sPhi = cPsi*TT[0][1]-sPsi*TT[1][1];
307 
308  PhiThePsi[0] = cPhi ; PhiThePsi[1] = sPhi;
309  PhiThePsi[2] = cThe ; PhiThePsi[3] = sThe;
310  PhiThePsi[4] = cPsi ; PhiThePsi[5] = sPsi;
311 
312 
313  double test = fabs(TT[0][0] -( cPsi*cPhi - sPsi*cThe*sPhi))
314  + fabs(TT[1][0] -(-sPsi*cPhi - cPsi*cThe*sPhi))
315  + fabs(TT[2][0] -( sThe*sPhi))
316  + fabs(TT[0][1] -( cPsi*sPhi + sPsi*cThe*cPhi))
317  + fabs(TT[1][1] -(-sPsi*sPhi + cPsi*cThe*cPhi))
318  + fabs(TT[2][1] -(-sThe*cPhi))
319  + fabs(TT[0][2] -( sPsi*sThe))
320  + fabs(TT[1][2] -( cPsi*sThe))
321  + fabs(TT[2][2] -( cThe));
322  printf("EPS=%g\n",test);
323 //
324 }
TT
Definition: T.h:18
Generated by   doxygen 1.8.5