31 long int ic, jc, it, jt;
37 long int NVar = (NTRK + 1) * 3;
48 for (it = 1; it <= NTRK; ++it) {
55 for (jt = 1; jt <= NTRK; ++jt) {
56 for (
int k = 0; k < 3; ++k) {
57 for (
int l = 0; l < 3; ++l) {
59 for (
int j = 0; j < 2; ++j) {
60 dpipj[k][l] += vk->
tmpArr[jt-1]->drdp[j][k] * drdpy[j][l];
64 for (
int k = 1; k <= 3; ++k) {
65 for (
int l = 1; l <= 3; ++l) {
66 ader_ref(it * 3 + k, jt * 3 + l) += dpipj[l-1][k-1];
85 for (it = 1; it <= NTRK; ++it) {
87 ader_ref(1, it * 3 + 1) += cnt * th0t.
X * tf0t.
X;
88 ader_ref(2, it * 3 + 1) += cnt * th0t.
Y * tf0t.
X;
89 ader_ref(3, it * 3 + 1) += cnt * th0t.
Z * tf0t.
X;
90 ader_ref(1, it * 3 + 2) += cnt * th0t.
X * tf0t.
Y;
91 ader_ref(2, it * 3 + 2) += cnt * th0t.
Y * tf0t.
Y;
92 ader_ref(3, it * 3 + 2) += cnt * th0t.
Z * tf0t.
Y;
93 ader_ref(1, it * 3 + 3) += cnt * th0t.
X * tf0t.
Z;
94 ader_ref(2, it * 3 + 3) += cnt * th0t.
Y * tf0t.
Z;
95 ader_ref(3, it * 3 + 3) += cnt * th0t.
Z * tf0t.
Z;
103 for (it = 1; it <= NTRK; ++it) {
104 for (jt = it; jt <= NTRK; ++jt) {
107 ader_ref(it*3 + 1, jt*3 + 1) += cnt * tf0ti.
X * tf0tj.
X;
108 ader_ref(it*3 + 2, jt*3 + 1) += cnt * tf0ti.
Y * tf0tj.
X;
109 ader_ref(it*3 + 3, jt*3 + 1) += cnt * tf0ti.
Z * tf0tj.
X;
110 ader_ref(it*3 + 1, jt*3 + 2) += cnt * tf0ti.
X * tf0tj.
Y;
111 ader_ref(it*3 + 2, jt*3 + 2) += cnt * tf0ti.
Y * tf0tj.
Y;
112 ader_ref(it*3 + 3, jt*3 + 2) += cnt * tf0ti.
Z * tf0tj.
Y;
113 ader_ref(it*3 + 1, jt*3 + 3) += cnt * tf0ti.
X * tf0tj.
Z;
114 ader_ref(it*3 + 2, jt*3 + 3) += cnt * tf0ti.
Y * tf0tj.
Z;
115 ader_ref(it*3 + 3, jt*3 + 3) += cnt * tf0ti.
Z * tf0tj.
Z;
121 for (i=1; i<=NVar-1; ++i) {
122 for (j = i+1; j<=NVar; ++j) {
145 for(i=0; i<NVar+1; i++){ ta[i] = tab.data() + i*(NVar+1);}
146 for (i=1; i<=NVar; ++i)
for (j = i; j<=NVar; ++j) ta[i][j] = ta[j][i] =
ader_ref(i,j);
154 for(i=0; i<NVar+1; i++){ tv[i] = tvb.data() + i*(NVar+1); tr[i] = trb.data() + i*(NVar+1);}
156 vkSVDCmp( ta.data(), NVar, NVar, tw.data(), tv.data());
159 for(i=1; i<NVar+1; i++)
if(fabs(tw[i])>tmax)tmax=fabs(tw[i]);
160 for(i=1; i<NVar+1; i++)
if(fabs(tw[i])/tmax < 1.e-18) tw[i]=0.;
161 for(i=1; i<=NVar; i++){
for(j=1; j<=NVar; j++){
162 tr[i][j]=0.;
for(
int k=1; k<=NVar; k++)
if(tw[k]!=0.) tr[i][j] += ta[i][k]*tv[j][k]/tw[k];
165 for (i=1; i<=NVar; ++i)
for (j=1; j<=NVar; ++j)
ader_ref(i,j)=tr[i][j];
171 for( i=1; i<=NVar; i++){
for(j=i; j<=NVar; j++){
172 double mcheck=0.;
for(
int k=1; k<=NVar; k++) mcheck+=ta[i][k]*
ader_ref(k,j);
173 if(i!=j) maxDiff = (maxDiff > std::abs(mcheck)) ? maxDiff : std::abs(mcheck);
174 if(i==j) maxDiff = (maxDiff > std::abs(1.-mcheck)) ? maxDiff : std::abs(1.-mcheck);
177 if(maxDiff>0.1)
return -1;
182 for (i=1; i<=NVar; i++) {
183 for (j=1; j<=NVar; j++) {
198 for (it=1; it<=NTRK; it++) {
199 t_trk=vk->
tmpArr[it-1].get();
201 - vcov[1] * t_trk->
wbci[1]
202 - vcov[3] * t_trk->
wbci[2];
204 - vcov[2] * t_trk->
wbci[1]
205 - vcov[4] * t_trk->
wbci[2];
207 - vcov[4] * t_trk->
wbci[1]
208 - vcov[5] * t_trk->
wbci[2];
210 - vcov[1] * t_trk->
wbci[4]
211 - vcov[3] * t_trk->
wbci[5];
213 - vcov[2] * t_trk->
wbci[4]
214 - vcov[4] * t_trk->
wbci[5];
216 - vcov[4] * t_trk->
wbci[4]
217 - vcov[5] * t_trk->
wbci[5];
219 - vcov[1] * t_trk->
wbci[7]
220 - vcov[3] * t_trk->
wbci[8];
222 - vcov[2] * t_trk->
wbci[7]
223 - vcov[4] * t_trk->
wbci[8];
225 - vcov[4] * t_trk->
wbci[7]
226 - vcov[5] * t_trk->
wbci[8];
239 for (it = 1; it<=NTRK; ++it) {
240 t_trk=vk->
tmpArr[it-1].get();
241 for (jt=1; jt<=NTRK; ++jt) {
273 std::vector<std::vector< Vect3DF> > tf0t;
274 std::vector< Vect3DF > th0t;
275 std::vector< double > taa;
276 std::vector< Vect3DF > tmpVec;
286 tf0t.push_back( tmpVec );
290 const std::size_t nConstraints = totNC;
291 const std::size_t nVar = NVar;
292 std::vector<double> R(nConstraints * nVar);
293 std::vector<double> RC(nConstraints * nVar);
294 std::vector<double> RCRt(nConstraints * nConstraints);
295 const auto index = [nVar](std::size_t row, std::size_t col){
296 return row * nVar + col;
299 for(ic=0; ic<totNC; ic++){
300 R[
index(ic, 0)]=th0t[ic].X;
301 R[
index(ic, 1)]=th0t[ic].Y;
302 R[
index(ic, 2)]=th0t[ic].Z;
303 for(
int it=1; it<=NTRK; it++){
304 R[
index(ic, it*3)]=tf0t[ic][it-1].X;
305 R[
index(ic, it*3+1)]=tf0t[ic][it-1].Y;
306 R[
index(ic, it*3+2)]=tf0t[ic][it-1].Z;
310 for(std::size_t ic=0; ic<nConstraints; ic++){
311 for(std::size_t j=0; j<nVar; j++){
313 for(std::size_t i=0; i<nVar; i++){
319 for(std::size_t ic=0; ic<nConstraints; ic++){
320 for(std::size_t jc=0; jc<nConstraints; jc++){
321 RCRt[ic*nConstraints + jc] =0.;
322 for(std::size_t i=0; i<nVar; i++){
323 RCRt[ic*nConstraints + jc] += RC[
index(ic, i)]*R[
index(jc, i)];
327 dsinv(totNC, RCRt.data(), totNC, &IERR);
328 if ( IERR != 0)
return IERR;
330 for(i=0; i<NVar; i++){
331 for(j=0; j<NVar; j++){
double COR=0.;
332 for(ic=0; ic<totNC; ic++){
333 for(jc=0; jc<totNC; jc++){
334 COR += RC[
index(ic,i)]*RC[
index(ic,j)]*RCRt[ic*nConstraints+jc];
353 for (i = 1; i <= 6; ++i) {
354 for (j = 1; j <= 6; ++j) {
356 for (ic=1; ic<=NVar; ++ic) {
357 if(
dcv_ref(i, ic)==0.)
continue;
358 for (jc=1; jc<=NVar; ++jc) {
359 if(
dcv_ref(j, jc)==0.)
continue;