00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #ifndef SI_PIXEL_TEMPLATE_STANDALONE
00015
00016 #include "RecoLocalTracker/SiPixelRecHits/interface/VVIObjF.h"
00017 #else
00018 #include "VVIObjF.h"
00019 #endif
00020
00021
00022 #include <cmath>
00023 #include <algorithm>
00024 #include "vdt/vdtMath.h"
00025
00026 namespace VVIObjFDetails {
00027 void sincosint(float x, float & sint, float & cint);
00028 float expint(float x);
00029
00030 template<typename F>
00031 int dzero(float a, float b, float& x0,
00032 float& rv, float eps, int mxf, F func);
00033 }
00034
00035
00036
00037
00043
00044
00045 VVIObjF::VVIObjF(float kappa, float beta2, int mode) : mode_(mode) {
00046
00047 const float xp[9] = { 9.29,2.47,.89,.36,.15,.07,.03,.02,0.0 };
00048 const float xq[7] = { .012,.03,.08,.26,.87,3.83,11.0 };
00049 float h_[7];
00050 float q, u, x, c1, c2, c3, c4, d1, h4, h5, h6, q2, x1, d, ll, ul, xf1, xf2, rv;
00051 int lp, lq, k, l, n;
00052
00053
00054
00055 if(kappa < 0.01f) kappa = 0.01f;
00056 if(kappa > 10.f) kappa = 10.f;
00057 if(beta2 < 0.f) beta2 = 0.f;
00058 if(beta2 > 1.f) beta2 = 1.f;
00059
00060 float invKappa = 1.f/kappa;
00061 h_[4] = 1.f - beta2*0.42278433999999998f + (7.6f*invKappa);
00062 h_[5] = beta2;
00063 h_[6] = 1.f - beta2;
00064 h4 = - (7.6f*invKappa) - (beta2 * .57721566f + 1.f);
00065 h5 = vdt::fast_logf(kappa);
00066 h6 = invKappa;
00067 t0_ = (h4 - h_[4]*h5 - (h_[4] + beta2)*(vdt::fast_logf(h_[4]) + VVIObjFDetails::expint(h_[4])) + vdt::fast_expf(-h_[4]))/h_[4];
00068
00069
00070
00071 for (lp = 0; lp < 9; ++lp) {
00072 if (kappa >= xp[lp]) break;
00073 }
00074 ll = -float(lp) - 1.5f;
00075 for (lq = 0; lq < 7; ++lq) {
00076 if (kappa <= xq[lq]) break;
00077 }
00078 ul = lq - 6.5f;
00079 auto f2 = [h_](float x) { return h_[4]-x+h_[5]*(vdt::fast_logf(std::abs(x))+VVIObjFDetails::expint(x))-h_[6]*vdt::fast_expf(-x);};
00080 VVIObjFDetails::dzero(ll, ul, u, rv, 1.e-3f, 100, f2);
00081 q = 1./u;
00082 t1_ = h4 * q - h5 - (beta2 * q + 1.f) * (vdt::fast_logf((fabs(u))) + VVIObjFDetails::expint(u)) + vdt::fast_expf(-u) * q;
00083 t_ = t1_ - t0_;
00084 omega_ = 6.2831853000000004f/t_;
00085 h_[0] = kappa * (beta2 * .57721566f + 2.f) + 9.9166128600000008f;
00086 if (kappa >= .07) {h_[0] += 6.90775527f;}
00087 h_[1] = beta2 * kappa;
00088 h_[2] = h6 * omega_;
00089 h_[3] = omega_ * 1.5707963250000001f;
00090 auto f1 = [h_](float x){ return h_[0]+h_[1]*vdt::fast_logf(h_[2]*x)-h_[3]*x;};
00091 VVIObjFDetails::dzero(5.f, 155.f, x0_, rv, 1.e-3f, 100, f1);
00092 n = x0_ + 1.;
00093 d = vdt::fast_expf(kappa * (beta2 * (.57721566f - h5) + 1.f)) * .31830988654751274f;
00094 a_[n - 1] = 0.f;
00095 if (mode_ == 0) {
00096 a_[n - 1] = omega_ * .31830988654751274f;
00097 }
00098 q = -1.;
00099 q2 = 2.;
00100 for (k = 1; k < n; ++k) {
00101 l = n - k;
00102 x = omega_ * k;
00103 x1 = h6 * x;
00104 VVIObjFDetails::sincosint(x1,c2,c1);
00105 c1 = vdt::fast_logf(x) - c1;
00106 vdt::fast_sincosf(x1,c3,c4);
00107 xf1 = kappa * (beta2 * c1 - c4) - x * c2;
00108 xf2 = x * c1 + kappa * (c3 + beta2 * c2) + t0_ * x;
00109 float s,c; vdt::fast_sincosf(xf2,s,c);
00110 if (mode_ == 0) {
00111 d1 = q * d * omega_ * vdt::fast_expf(xf1);
00112 a_[l - 1] = d1 * c;
00113 b_[l - 1] = -d1 * s;
00114 } else {
00115 d1 = q * d * vdt::fast_expf(xf1)/k;
00116 a_[l - 1] = d1 * s;
00117 b_[l - 1] = d1 * c;
00118 a_[n - 1] += q2 * a_[l - 1];
00119 }
00120 q = -q;
00121 q2 = -q2;
00122 }
00123
00124 }
00125
00126
00130
00131
00132
00133 float VVIObjF::fcn(float x) const {
00134
00135
00136
00137 float f, u, y, a0, a1;
00138 float a2 = 0.;
00139 float b1, b0, b2, cof;
00140 int k, n, n1;
00141
00142 n = x0_;
00143 if (x < t0_) {
00144 f = 0.f;
00145 } else if (x <= t1_) {
00146 y = x - t0_;
00147 u = omega_ * y - 3.141592653589793f;
00148 float su,cu; vdt::fast_sincosf(u,su,cu);
00149 cof = cu * 2.f;
00150 a1 = 0.;
00151 a0 = a_[0];
00152 n1=n+1;
00153 for (k = 2; k <= n1; ++k) {
00154 a2 = a1;
00155 a1 = a0;
00156 a0 = a_[k - 1] + cof * a1 - a2;
00157 }
00158 b1 = 0.;
00159 b0 = b_[0];
00160 for (k = 2; k <= n; ++k) {
00161 b2 = b1;
00162 b1 = b0;
00163 b0 = b_[k - 1] + cof * b1 - b2;
00164 }
00165 f = (a0 - a2) * .5f + b0 * su;
00166 if (mode_ != 0) {f += y / t_;}
00167 } else {
00168 f = 0.f;
00169 if (mode_ != 0) {f = 1.f;}
00170 }
00171 return f;
00172 }
00173
00174
00175
00176
00180
00181
00182
00183 void VVIObjF::limits(float& xl, float& xu) const {
00184
00185 xl = t0_;
00186 xu = t1_;
00187 return;
00188 }
00189
00190
00191 #include "sicif.h"
00192 namespace VVIObjFDetails {
00193 void sincosint(float x, float & sint, float & cint) {
00194 sicif(x,sint,cint);
00195 }
00196
00197
00198 float expint(float x) {
00199
00200
00201
00202 const float zero = 0.;
00203 const float q2[7] = { .10340013040487,3.319092135933,
00204 20.449478501379,41.280784189142,32.426421069514,10.041164382905,
00205 1. };
00206 const float p3[6] = { -2.3909964453136,-147.98219500504,
00207 -254.3763397689,-119.55761038372,-19.630408535939,-.9999999999036
00208 };
00209 const float q3[6] = { 177.60070940351,530.68509610812,
00210 462.23027156148,156.81843364539,21.630408494238,1. };
00211 const float p4[8] = { -8.6693733995107,-549.14226552109,
00212 -4210.0161535707,-249301.39345865,-119623.66934925,
00213 -22174462.775885,3892804.213112,-391546073.8091 };
00214 const float q4[8] = { 34.171875,-1607.0892658722,35730.029805851,
00215 -483547.43616216,4285596.2461175,-24903337.574054,89192576.757561,
00216 -165254299.72521 };
00217 const float a1[8] = { -2.1808638152072,-21.901023385488,
00218 9.3081638566217,25.076281129356,-33.184253199722,60.121799083008,
00219 -43.253113287813,1.0044310922808 };
00220 const float b1[8] = { 0.,3.9370770185272,300.89264837292,
00221 -6.2504116167188,1003.6743951673,14.325673812194,2736.2411988933,
00222 .52746885196291 };
00223 const float a2[8] = { -3.4833465360285,-18.65454548834,
00224 -8.2856199414064,-32.34673303054,17.960168876925,1.7565631546961,
00225 -1.9502232128966,.99999429607471 };
00226 const float b2[8] = { 0.,69.500065588743,57.283719383732,
00227 25.777638423844,760.76114800773,28.951672792514,-3.4394226689987,
00228 1.0008386740264 };
00229 const float a3[6] = { -27.780928934438,-10.10479081576,
00230 -9.1483008216736,-5.0223317461851,-3.0000077799358,
00231 1.0000000000704 };
00232 const float one = 1.;
00233 const float b3[6] = { 0.,122.39993926823,2.7276100778779,
00234 -7.1897518395045,-2.9990118065262,1.999999942826 };
00235 const float two = 2.;
00236 const float three = 3.;
00237 const float x0 = .37250741078137;
00238 const float xl[6] = { -24.,-12.,-6.,0.,1.,4. };
00239 const float p1[5] = { 4.293125234321,39.894153870321,
00240 292.52518866921,425.69682638592,-434.98143832952 };
00241 const float q1[5] = { 1.,18.899288395003,150.95038744251,
00242 568.05252718987,753.58564359843 };
00243 const float p2[7] = { .43096783946939,6.9052252278444,
00244 23.019255939133,24.378408879132,9.0416155694633,.99997957705159,
00245 4.656271079751e-7 };
00246
00247
00248 float v, y, ap, bp, aq, dp, bq, dq;
00249
00250 if (x <= xl[0]) {
00251 ap = a3[0] - x;
00252 for ( int i__ = 2; i__ <= 5; ++i__) {
00253 ap = a3[i__ - 1] - x + b3[i__ - 1] / ap;
00254 }
00255 y = vdt::fast_expf(-x) / x * (one - (a3[5] + b3[5] / ap) / x);
00256 } else if (x <= xl[1]) {
00257 ap = a2[0] - x;
00258 for ( int i__ = 2; i__ <= 7; ++i__) {
00259 ap = a2[i__ - 1] - x + b2[i__ - 1] / ap;
00260 }
00261 y = vdt::fast_expf(-x) / x * (a2[7] + b2[7] / ap);
00262 } else if (x <= xl[2]) {
00263 ap = a1[0] - x;
00264 for ( int i__ = 2; i__ <= 7; ++i__) {
00265 ap = a1[i__ - 1] - x + b1[i__ - 1] / ap;
00266 }
00267 y = vdt::fast_expf(-x) / x * (a1[7] + b1[7] / ap);
00268 } else if (x < xl[3]) {
00269 v = -two * (x / three + one);
00270 bp = zero;
00271 dp = p4[0];
00272 for ( int i__ = 2; i__ <= 8; ++i__) {
00273 ap = bp;
00274 bp = dp;
00275 dp = p4[i__ - 1] - ap + v * bp;
00276 }
00277 bq = zero;
00278 dq = q4[0];
00279 for ( int i__ = 2; i__ <= 8; ++i__) {
00280 aq = bq;
00281 bq = dq;
00282 dq = q4[i__ - 1] - aq + v * bq;
00283 }
00284 y = -vdt::fast_logf(-x / x0) + (x + x0) * (dp - ap) / (dq - aq);
00285 } else if (x == xl[3]) {
00286 return zero;
00287 } else if (x < xl[4]) {
00288 ap = p1[0];
00289 aq = q1[0];
00290 for ( int i__ = 2; i__ <= 5; ++i__) {
00291 ap = p1[i__ - 1] + x * ap;
00292 aq = q1[i__ - 1] + x * aq;
00293 }
00294 y = -vdt::fast_logf(x) + ap / aq;
00295 } else if (x <= xl[5]) {
00296 y = one / x;
00297 ap = p2[0];
00298 aq = q2[0];
00299 for ( int i__ = 2; i__ <= 7; ++i__) {
00300 ap = p2[i__ - 1] + y * ap;
00301 aq = q2[i__ - 1] + y * aq;
00302 }
00303 y = vdt::fast_expf(-x) * ap / aq;
00304 } else {
00305 y = one / x;
00306 ap = p3[0];
00307 aq = q3[0];
00308 for ( int i__ = 2; i__ <= 6; ++i__) {
00309 ap = p3[i__ - 1] + y * ap;
00310 aq = q3[i__ - 1] + y * aq;
00311 }
00312 y = vdt::fast_expf(-x) * y * (one + y * ap / aq);
00313 }
00314 return y;
00315 }
00316
00317
00318
00319
00320 template<typename F>
00321 int dzero(float a, float b, float& x0,
00322 float& rv, float eps, int mxf, F func) {
00323
00324 float d__1, d__2, d__3, d__4;
00325
00326
00327 float f1, f2, f3, u1, u2, x1, x2, u3, u4, x3, ca, cb, cc, fa, fb, ee, ff;
00328 int mc;
00329 float xa, xb, fx, xx, su4;
00330
00331 xa = std::min(a,b);
00332 xb = std::max(a,b);
00333 fa = func(xa);
00334 fb = func(xb);
00335 if (fa * fb > 0.f) {
00336 rv = (xb - xa) * -2.f;
00337 x0 = 0.f;
00338 return 1;
00339 }
00340 mc = 0;
00341 L1:
00342 x0 = (xa + xb) * 0.5f;
00343 rv = x0 - xa;
00344 ee = eps * (std::abs(x0) + 1.f);
00345 if (rv <= ee) {
00346 rv = ee;
00347 ff = func(x0);
00348 return 0;
00349 }
00350 f1 = fa;
00351 x1 = xa;
00352 f2 = fb;
00353 x2 = xb;
00354 L2:
00355 fx = func(x0);
00356 ++mc;
00357 if (mc > mxf) {
00358 rv = (d__1 = xb - xa, fabs(d__1)) * -0.5f;
00359 x0 = 0.;
00360 return 0;
00361 }
00362 if (fx * fa > 0.f) {
00363 xa = x0;
00364 fa = fx;
00365 } else {
00366 xb = x0;
00367 fb = fx;
00368 }
00369 L3:
00370 u1 = f1 - f2;
00371 u2 = x1 - x2;
00372 u3 = f2 - fx;
00373 u4 = x2 - x0;
00374 if (u2 == 0.f || u4 == 0.f) {goto L1;}
00375 f3 = fx;
00376 x3 = x0;
00377 u1 /= u2;
00378 u2 = u3 / u4;
00379 ca = u1 - u2;
00380 cb = (x1 + x2) * u2 - (x2 + x0) * u1;
00381 cc = (x1 - x0) * f1 - x1 * (ca * x1 + cb);
00382 if (ca == 0.f) {
00383 if (cb == 0.f) {goto L1;}
00384 x0 = -cc / cb;
00385 } else {
00386 u3 = cb / (ca * 2.f);
00387 u4 = u3 * u3 - cc / ca;
00388 if (u4 < 0.f) {goto L1;}
00389 su4 = std::abs(u4);
00390 if (x0 + u3 < 0.f) {su4 = -su4;}
00391 x0 = -u3 + su4;
00392 }
00393 if (x0 < xa || x0 > xb) {goto L1;}
00394
00395 d__3 = (d__1 = x0 - x3, std::abs(d__1));
00396 d__4 = (d__2 = x0 - x2, std::abs(d__2));
00397 rv = std::min(d__3,d__4);
00398 ee = eps * (std::abs(x0) + 1);
00399 if (rv > ee) {
00400 f1 = f2;
00401 x1 = x2;
00402 f2 = f3;
00403 x2 = x3;
00404 goto L2;
00405 }
00406 fx = func(x0);
00407 if (fx == 0.f) {
00408 rv = ee;
00409 ff = func(x0);
00410 return 0;
00411 }
00412 if (fx * fa < 0.f) {
00413 xx = x0 - ee;
00414 if (xx <= xa) {
00415 rv = ee;
00416 ff = func(x0);
00417 return 0;
00418 }
00419 ff = func(xx);
00420 fb = ff;
00421 xb = xx;
00422 } else {
00423 xx = x0 + ee;
00424 if (xx >= xb) {
00425 rv = ee;
00426 ff = func(x0);
00427 return 0;
00428 }
00429 ff = func(xx);
00430 fa = ff;
00431 xa = xx;
00432 }
00433 if (fx * ff > 0.f) {
00434 mc += 2;
00435 if (mc > mxf) {
00436 rv = (d__1 = xb - xa, std::abs(d__1)) * -0.5f;
00437 x0 = 0.f;
00438 return 0;
00439 }
00440 f1 = f3;
00441 x1 = x3;
00442 f2 = fx;
00443 x2 = x0;
00444 x0 = xx;
00445 fx = ff;
00446 goto L3;
00447 }
00448
00449 rv = ee;
00450 ff = func(x0);
00451 return 0;
00452 }
00453
00454 }