File indexing completed on 2024-04-06 12:26:26
0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015 #ifndef SI_PIXEL_TEMPLATE_STANDALONE
0016
0017 #include "RecoLocalTracker/SiPixelRecHits/interface/VVIObjF.h"
0018 #else
0019 #include "VVIObjF.h"
0020 #endif
0021
0022 #include <cmath>
0023 #include <algorithm>
0024 #include "vdt/vdtMath.h"
0025
0026 namespace VVIObjFDetails {
0027 void sincosint(float x, float& sint, float& cint);
0028 float expint(float x);
0029
0030 template <typename F>
0031 int dzero(float a, float b, float& x0, float& rv, float eps, int mxf, F func);
0032 }
0033
0034
0035
0036
0037
0038
0039
0040
0041
0042
0043 VVIObjF::VVIObjF(float kappa, float beta2, int mode) : mode_(mode) {
0044 const float xp[9] = {9.29, 2.47, .89, .36, .15, .07, .03, .02, 0.0};
0045 const float xq[7] = {.012, .03, .08, .26, .87, 3.83, 11.0};
0046 float h_[7];
0047 float q, u, h4, h5, h6, q2, d, ll, ul, rv;
0048 int lp, lq, k, l, n;
0049
0050
0051
0052 if (kappa < 0.01f)
0053 kappa = 0.01f;
0054 if (kappa > 10.f)
0055 kappa = 10.f;
0056 if (beta2 < 0.f)
0057 beta2 = 0.f;
0058 if (beta2 > 1.f)
0059 beta2 = 1.f;
0060
0061 float invKappa = 1.f / kappa;
0062 h_[4] = 1.f - beta2 * 0.42278433999999998f + (7.6f * invKappa);
0063 h_[5] = beta2;
0064 h_[6] = 1.f - beta2;
0065 h4 = -(7.6f * invKappa) - (beta2 * .57721566f + 1.f);
0066 h5 = vdt::fast_logf(kappa);
0067 h6 = invKappa;
0068 t0_ = (h4 - h_[4] * h5 - (h_[4] + beta2) * (vdt::fast_logf(h_[4]) + VVIObjFDetails::expint(h_[4])) +
0069 vdt::fast_expf(-h_[4])) /
0070 h_[4];
0071
0072
0073
0074 for (lp = 0; lp < 9; ++lp) {
0075 if (kappa >= xp[lp])
0076 break;
0077 }
0078 ll = -float(lp) - 1.5f;
0079 for (lq = 0; lq < 7; ++lq) {
0080 if (kappa <= xq[lq])
0081 break;
0082 }
0083 ul = lq - 6.5f;
0084 auto f2 = [h_](float x) {
0085 return h_[4] - x + h_[5] * (vdt::fast_logf(std::abs(x)) + VVIObjFDetails::expint(x)) - h_[6] * vdt::fast_expf(-x);
0086 };
0087 VVIObjFDetails::dzero(ll, ul, u, rv, 1.e-3f, 100, f2);
0088 q = 1. / u;
0089 t1_ = h4 * q - h5 - (beta2 * q + 1.f) * (vdt::fast_logf((fabs(u))) + VVIObjFDetails::expint(u)) +
0090 vdt::fast_expf(-u) * q;
0091 t_ = t1_ - t0_;
0092 omega_ = 6.2831853000000004f / t_;
0093 h_[0] = kappa * (beta2 * .57721566f + 2.f) + 9.9166128600000008f;
0094 if (kappa >= .07) {
0095 h_[0] += 6.90775527f;
0096 }
0097 h_[1] = beta2 * kappa;
0098 h_[2] = h6 * omega_;
0099 h_[3] = omega_ * 1.5707963250000001f;
0100 auto f1 = [h_](float x) { return h_[0] + h_[1] * vdt::fast_logf(h_[2] * x) - h_[3] * x; };
0101 VVIObjFDetails::dzero(5.f, 155.f, x0_, rv, 1.e-3f, 100, f1);
0102 n = x0_ + 1.;
0103 d = vdt::fast_expf(kappa * (beta2 * (.57721566f - h5) + 1.f)) * .31830988654751274f;
0104 a_[n - 1] = 0.f;
0105 if (mode_ == 0) {
0106 a_[n - 1] = omega_ * .31830988654751274f;
0107 }
0108 q = -1.;
0109 q2 = 2.;
0110
0111 float x[n];
0112 x[0] = 0.f;
0113 float x1[n];
0114 x1[0] = 0.f;
0115 float c1[n];
0116 c1[0] = 0.f;
0117 float c2[n];
0118 c2[0] = 0.f;
0119 float c3[n];
0120 c3[0] = 0.f;
0121 float c4[n];
0122 c4[0] = 0.f;
0123 float s[n];
0124 s[0] = 0.f;
0125 float c[n];
0126 c[0] = 0.f;
0127 float xf1[n];
0128 xf1[0] = 0.f;
0129 float xf2[n];
0130 xf2[0] = 0.f;
0131 for (k = 1; k < n; ++k) {
0132 x[k] = omega_ * k;
0133 x1[k] = h6 * x[k];
0134 }
0135 for (k = 1; k < n; ++k) {
0136 VVIObjFDetails::sincosint(x1[k], c2[k], c1[k]);
0137 }
0138 for (k = 1; k < n; ++k) {
0139 c1[k] = vdt::fast_logf(x[k]) - c1[k];
0140 }
0141 for (k = 1; k < n; ++k) {
0142 vdt::fast_sincosf(x1[k], c3[k], c4[k]);
0143 xf1[k] = kappa * (beta2 * c1[k] - c4[k]) - x[k] * c2[k];
0144 xf2[k] = x[k] * c1[k] + kappa * (c3[k] + beta2 * c2[k]) + t0_ * x[k];
0145 }
0146 for (k = 1; k < n; ++k) {
0147 vdt::fast_sincosf(xf2[k], s[k], c[k]);
0148 }
0149 float d1[n];
0150 d1[0] = 0.f;
0151 if (mode_ == 0) {
0152 for (k = 1; k < n; ++k) {
0153 d1[k] = q * d * omega_ * vdt::fast_expf(xf1[k]);
0154 q = -q;
0155 }
0156 for (k = 1; k < n; ++k) {
0157 l = n - k;
0158 a_[l - 1] = d1[k] * c[k];
0159 b_[l - 1] = -d1[k] * s[k];
0160 }
0161 } else {
0162 for (k = 1; k < n; ++k) {
0163 d1[k] = q * d * vdt::fast_expf(xf1[k]) / k;
0164 q = -q;
0165 }
0166 for (k = 1; k < n; ++k) {
0167 l = n - k;
0168 a_[l - 1] = d1[k] * s[k];
0169 }
0170 for (k = 1; k < n; ++k) {
0171 l = n - k;
0172 b_[l - 1] = d1[k] * c[k];
0173 }
0174 for (k = 1; k < n; ++k) {
0175 l = n - k;
0176 a_[n - 1] += q2 * a_[l - 1];
0177 q2 = -q2;
0178 }
0179 }
0180 }
0181
0182
0183
0184
0185
0186
0187
0188
0189 VVIObjF::VVIObjF(float kappa) : mode_(1) {
0190 const float xp[9] = {9.29, 2.47, .89, .36, .15, .07, .03, .02, 0.0};
0191 const float xq[7] = {.012, .03, .08, .26, .87, 3.83, 11.0};
0192 float h_[5];
0193 float q, u, h4, h5, h6, q2, d, ll, ul, rv;
0194 int lp, lq, k, l, n;
0195
0196
0197
0198 if (kappa < 0.01f)
0199 kappa = 0.01f;
0200 if (kappa > 10.f)
0201 kappa = 10.f;
0202
0203 float invKappa = 1.f / kappa;
0204 h_[4] = 0.57721566f + (7.6f * invKappa);
0205 h4 = -(7.6f * invKappa) - 1.57721566f;
0206 h5 = vdt::fast_logf(kappa);
0207 h6 = invKappa;
0208 t0_ = (h4 - h_[4] * h5 - (h_[4] + 1.f) * (vdt::fast_logf(h_[4]) + VVIObjFDetails::expint(h_[4])) +
0209 vdt::fast_expf(-h_[4])) /
0210 h_[4];
0211
0212
0213
0214 for (lp = 0; lp < 9; ++lp) {
0215 if (kappa >= xp[lp])
0216 break;
0217 }
0218 ll = -float(lp) - 1.5f;
0219 for (lq = 0; lq < 7; ++lq) {
0220 if (kappa <= xq[lq])
0221 break;
0222 }
0223 ul = lq - 6.5f;
0224 auto f2 = [h_](float x) { return h_[4] - x + (vdt::fast_logf(std::abs(x)) + VVIObjFDetails::expint(x)); };
0225 VVIObjFDetails::dzero(ll, ul, u, rv, 1.e-3f, 100, f2);
0226 q = 1. / u;
0227 t1_ = h4 * q - h5 - (q + 1.f) * (vdt::fast_logf((fabs(u))) + VVIObjFDetails::expint(u)) + vdt::fast_expf(-u) * q;
0228 t_ = t1_ - t0_;
0229 omega_ = 6.2831853000000004f / t_;
0230 h_[0] = kappa * 2.57721566f + 9.9166128600000008f;
0231 if (kappa >= .07) {
0232 h_[0] += 6.90775527f;
0233 }
0234 h_[1] = kappa;
0235 h_[2] = h6 * omega_;
0236 h_[3] = omega_ * 1.5707963250000001f;
0237 auto f1 = [h_](float x) { return h_[0] + h_[1] * vdt::fast_logf(h_[2] * x) - h_[3] * x; };
0238 VVIObjFDetails::dzero(5.f, 155.f, x0_, rv, 1.e-3f, 100, f1);
0239 n = x0_ + 1.;
0240 d = vdt::fast_expf(kappa * ((0.57721566f - h5) + 1.f)) * .31830988654751274f;
0241 a_[n - 1] = 0.f;
0242 q = -1.;
0243 q2 = 2.;
0244
0245 float x[n];
0246 x[0] = 0.f;
0247 float x1[n];
0248 x1[0] = 0.f;
0249 float c1[n];
0250 c1[0] = 0.f;
0251 float c2[n];
0252 c2[0] = 0.f;
0253 float c3[n];
0254 c3[0] = 0.f;
0255 float c4[n];
0256 c4[0] = 0.f;
0257 float s[n];
0258 s[0] = 0.f;
0259 float c[n];
0260 c[0] = 0.f;
0261 float xf1[n];
0262 xf1[0] = 0.f;
0263 float xf2[n];
0264 xf2[0] = 0.f;
0265 for (k = 1; k < n; ++k) {
0266 x[k] = omega_ * k;
0267 x1[k] = h6 * x[k];
0268 }
0269 for (k = 1; k < n; ++k) {
0270 VVIObjFDetails::sincosint(x1[k], c2[k], c1[k]);
0271 }
0272 for (k = 1; k < n; ++k) {
0273 c1[k] = vdt::fast_logf(x[k]) - c1[k];
0274 }
0275 for (k = 1; k < n; ++k) {
0276 vdt::fast_sincosf(x1[k], c3[k], c4[k]);
0277 xf1[k] = kappa * (c1[k] - c4[k]) - x[k] * c2[k];
0278 xf2[k] = x[k] * c1[k] + kappa * (c3[k] + c2[k]) + t0_ * x[k];
0279 }
0280 for (k = 1; k < n; ++k) {
0281 vdt::fast_sincosf(xf2[k], s[k], c[k]);
0282 }
0283 float d1[n];
0284 d1[0] = 0.f;
0285 for (k = 1; k < n; ++k) {
0286 d1[k] = q * d * vdt::fast_expf(xf1[k]) / k;
0287 q = -q;
0288 }
0289 for (k = 1; k < n; ++k) {
0290 l = n - k;
0291 a_[l - 1] = d1[k] * s[k];
0292 }
0293 for (k = 1; k < n; ++k) {
0294 l = n - k;
0295 b_[l - 1] = d1[k] * c[k];
0296 }
0297 for (k = 1; k < n; ++k) {
0298 l = n - k;
0299 a_[n - 1] += q2 * a_[l - 1];
0300 q2 = -q2;
0301 }
0302
0303 }
0304
0305
0306
0307
0308
0309
0310
0311 float VVIObjF::fcn(float x) const {
0312
0313
0314 float f, u, y, a0, a1;
0315 float a2 = 0.;
0316 float b1, b0, b2, cof;
0317 int k, n, n1;
0318
0319 n = x0_;
0320 if (x < t0_) {
0321 f = 0.f;
0322 } else if (x <= t1_) {
0323 y = x - t0_;
0324 u = omega_ * y - 3.141592653589793f;
0325 float su, cu;
0326 vdt::fast_sincosf(u, su, cu);
0327 cof = cu * 2.f;
0328 a1 = 0.;
0329 a0 = a_[0];
0330 n1 = n + 1;
0331 for (k = 2; k <= n1; ++k) {
0332 a2 = a1;
0333 a1 = a0;
0334 a0 = a_[k - 1] + cof * a1 - a2;
0335 }
0336 b1 = 0.;
0337 b0 = b_[0];
0338 for (k = 2; k <= n; ++k) {
0339 b2 = b1;
0340 b1 = b0;
0341 b0 = b_[k - 1] + cof * b1 - b2;
0342 }
0343 f = (a0 - a2) * .5f + b0 * su;
0344 if (mode_ != 0) {
0345 f += y / t_;
0346 }
0347 } else {
0348 f = 0.f;
0349 if (mode_ != 0) {
0350 f = 1.f;
0351 }
0352 }
0353 return f;
0354 }
0355
0356
0357
0358
0359
0360
0361
0362 void VVIObjF::limits(float& xl, float& xu) const {
0363 xl = t0_;
0364 xu = t1_;
0365 return;
0366 }
0367
0368 #include "sicif.h"
0369 namespace VVIObjFDetails {
0370 void sincosint(float x, float& sint, float& cint) { sicif(x, sint, cint); }
0371
0372 float expint(float x) {
0373
0374
0375 const float zero = 0.;
0376 const float q2[7] = {
0377 .10340013040487, 3.319092135933, 20.449478501379, 41.280784189142, 32.426421069514, 10.041164382905, 1.};
0378 const float p3[6] = {
0379 -2.3909964453136, -147.98219500504, -254.3763397689, -119.55761038372, -19.630408535939, -.9999999999036};
0380 const float q3[6] = {177.60070940351, 530.68509610812, 462.23027156148, 156.81843364539, 21.630408494238, 1.};
0381 const float p4[8] = {-8.6693733995107,
0382 -549.14226552109,
0383 -4210.0161535707,
0384 -249301.39345865,
0385 -119623.66934925,
0386 -22174462.775885,
0387 3892804.213112,
0388 -391546073.8091};
0389 const float q4[8] = {34.171875,
0390 -1607.0892658722,
0391 35730.029805851,
0392 -483547.43616216,
0393 4285596.2461175,
0394 -24903337.574054,
0395 89192576.757561,
0396 -165254299.72521};
0397 const float a1[8] = {-2.1808638152072,
0398 -21.901023385488,
0399 9.3081638566217,
0400 25.076281129356,
0401 -33.184253199722,
0402 60.121799083008,
0403 -43.253113287813,
0404 1.0044310922808};
0405 const float b1[8] = {0.,
0406 3.9370770185272,
0407 300.89264837292,
0408 -6.2504116167188,
0409 1003.6743951673,
0410 14.325673812194,
0411 2736.2411988933,
0412 .52746885196291};
0413 const float a2[8] = {-3.4833465360285,
0414 -18.65454548834,
0415 -8.2856199414064,
0416 -32.34673303054,
0417 17.960168876925,
0418 1.7565631546961,
0419 -1.9502232128966,
0420 .99999429607471};
0421 const float b2[8] = {0.,
0422 69.500065588743,
0423 57.283719383732,
0424 25.777638423844,
0425 760.76114800773,
0426 28.951672792514,
0427 -3.4394226689987,
0428 1.0008386740264};
0429 const float a3[6] = {
0430 -27.780928934438, -10.10479081576, -9.1483008216736, -5.0223317461851, -3.0000077799358, 1.0000000000704};
0431 const float one = 1.;
0432 const float b3[6] = {0., 122.39993926823, 2.7276100778779, -7.1897518395045, -2.9990118065262, 1.999999942826};
0433 const float two = 2.;
0434 const float three = 3.;
0435 const float x0 = .37250741078137;
0436 const float xl[6] = {-24., -12., -6., 0., 1., 4.};
0437 const float p1[5] = {4.293125234321, 39.894153870321, 292.52518866921, 425.69682638592, -434.98143832952};
0438 const float q1[5] = {1., 18.899288395003, 150.95038744251, 568.05252718987, 753.58564359843};
0439 const float p2[7] = {.43096783946939,
0440 6.9052252278444,
0441 23.019255939133,
0442 24.378408879132,
0443 9.0416155694633,
0444 .99997957705159,
0445 4.656271079751e-7};
0446
0447
0448 float v, y, ap, bp, aq, dp, bq, dq;
0449
0450 if (x <= xl[0]) {
0451 ap = a3[0] - x;
0452 for (int i__ = 2; i__ <= 5; ++i__) {
0453 ap = a3[i__ - 1] - x + b3[i__ - 1] / ap;
0454 }
0455 y = vdt::fast_expf(-x) / x * (one - (a3[5] + b3[5] / ap) / x);
0456 } else if (x <= xl[1]) {
0457 ap = a2[0] - x;
0458 for (int i__ = 2; i__ <= 7; ++i__) {
0459 ap = a2[i__ - 1] - x + b2[i__ - 1] / ap;
0460 }
0461 y = vdt::fast_expf(-x) / x * (a2[7] + b2[7] / ap);
0462 } else if (x <= xl[2]) {
0463 ap = a1[0] - x;
0464 for (int i__ = 2; i__ <= 7; ++i__) {
0465 ap = a1[i__ - 1] - x + b1[i__ - 1] / ap;
0466 }
0467 y = vdt::fast_expf(-x) / x * (a1[7] + b1[7] / ap);
0468 } else if (x < xl[3]) {
0469 v = -two * (x / three + one);
0470 bp = zero;
0471 dp = p4[0];
0472 for (int i__ = 2; i__ <= 8; ++i__) {
0473 ap = bp;
0474 bp = dp;
0475 dp = p4[i__ - 1] - ap + v * bp;
0476 }
0477 bq = zero;
0478 dq = q4[0];
0479 for (int i__ = 2; i__ <= 8; ++i__) {
0480 aq = bq;
0481 bq = dq;
0482 dq = q4[i__ - 1] - aq + v * bq;
0483 }
0484 y = -vdt::fast_logf(-x / x0) + (x + x0) * (dp - ap) / (dq - aq);
0485 } else if (x == xl[3]) {
0486 return zero;
0487 } else if (x < xl[4]) {
0488 ap = p1[0];
0489 aq = q1[0];
0490 for (int i__ = 2; i__ <= 5; ++i__) {
0491 ap = p1[i__ - 1] + x * ap;
0492 aq = q1[i__ - 1] + x * aq;
0493 }
0494 y = -vdt::fast_logf(x) + ap / aq;
0495 } else if (x <= xl[5]) {
0496 y = one / x;
0497 ap = p2[0];
0498 aq = q2[0];
0499 for (int i__ = 2; i__ <= 7; ++i__) {
0500 ap = p2[i__ - 1] + y * ap;
0501 aq = q2[i__ - 1] + y * aq;
0502 }
0503 y = vdt::fast_expf(-x) * ap / aq;
0504 } else {
0505 y = one / x;
0506 ap = p3[0];
0507 aq = q3[0];
0508 for (int i__ = 2; i__ <= 6; ++i__) {
0509 ap = p3[i__ - 1] + y * ap;
0510 aq = q3[i__ - 1] + y * aq;
0511 }
0512 y = vdt::fast_expf(-x) * y * (one + y * ap / aq);
0513 }
0514 return y;
0515 }
0516
0517 template <typename F>
0518 int dzero(float a, float b, float& x0, float& rv, float eps, int mxf, F func) {
0519
0520 float d__1, d__2, d__3, d__4;
0521
0522
0523 float f1, f2, f3, u1, u2, x1, x2, u3, u4, x3, ca, cb, cc, fa, fb, ee, ff;
0524 int mc;
0525 float xa, xb, fx, xx, su4;
0526
0527 xa = std::min(a, b);
0528 xb = std::max(a, b);
0529 fa = func(xa);
0530 fb = func(xb);
0531 if (fa * fb > 0.f) {
0532 rv = (xb - xa) * -2.f;
0533 x0 = 0.f;
0534 return 1;
0535 }
0536 mc = 0;
0537 L1:
0538 x0 = (xa + xb) * 0.5f;
0539 rv = x0 - xa;
0540 ee = eps * (std::abs(x0) + 1.f);
0541 if (rv <= ee) {
0542 rv = ee;
0543 ff = func(x0);
0544 return 0;
0545 }
0546 f1 = fa;
0547 x1 = xa;
0548 f2 = fb;
0549 x2 = xb;
0550 L2:
0551 fx = func(x0);
0552 ++mc;
0553 if (mc > mxf) {
0554 rv = (d__1 = xb - xa, fabs(d__1)) * -0.5f;
0555 x0 = 0.;
0556 return 0;
0557 }
0558 if (fx * fa > 0.f) {
0559 xa = x0;
0560 fa = fx;
0561 } else {
0562 xb = x0;
0563 fb = fx;
0564 }
0565 L3:
0566 u1 = f1 - f2;
0567 u2 = x1 - x2;
0568 u3 = f2 - fx;
0569 u4 = x2 - x0;
0570 if (u2 == 0.f || u4 == 0.f) {
0571 goto L1;
0572 }
0573 f3 = fx;
0574 x3 = x0;
0575 u1 /= u2;
0576 u2 = u3 / u4;
0577 ca = u1 - u2;
0578 cb = (x1 + x2) * u2 - (x2 + x0) * u1;
0579 cc = (x1 - x0) * f1 - x1 * (ca * x1 + cb);
0580 if (ca == 0.f) {
0581 if (cb == 0.f) {
0582 goto L1;
0583 }
0584 x0 = -cc / cb;
0585 } else {
0586 u3 = cb / (ca * 2.f);
0587 u4 = u3 * u3 - cc / ca;
0588 if (u4 < 0.f) {
0589 goto L1;
0590 }
0591 su4 = std::abs(u4);
0592 if (x0 + u3 < 0.f) {
0593 su4 = -su4;
0594 }
0595 x0 = -u3 + su4;
0596 }
0597 if (x0 < xa || x0 > xb) {
0598 goto L1;
0599 }
0600
0601 d__3 = (d__1 = x0 - x3, std::abs(d__1));
0602 d__4 = (d__2 = x0 - x2, std::abs(d__2));
0603 rv = std::min(d__3, d__4);
0604 ee = eps * (std::abs(x0) + 1);
0605 if (rv > ee) {
0606 f1 = f2;
0607 x1 = x2;
0608 f2 = f3;
0609 x2 = x3;
0610 goto L2;
0611 }
0612 fx = func(x0);
0613 if (fx == 0.f) {
0614 rv = ee;
0615 ff = func(x0);
0616 return 0;
0617 }
0618 if (fx * fa < 0.f) {
0619 xx = x0 - ee;
0620 if (xx <= xa) {
0621 rv = ee;
0622 ff = func(x0);
0623 return 0;
0624 }
0625 ff = func(xx);
0626 fb = ff;
0627 xb = xx;
0628 } else {
0629 xx = x0 + ee;
0630 if (xx >= xb) {
0631 rv = ee;
0632 ff = func(x0);
0633 return 0;
0634 }
0635 ff = func(xx);
0636 fa = ff;
0637 xa = xx;
0638 }
0639 if (fx * ff > 0.f) {
0640 mc += 2;
0641 if (mc > mxf) {
0642 rv = (d__1 = xb - xa, std::abs(d__1)) * -0.5f;
0643 x0 = 0.f;
0644 return 0;
0645 }
0646 f1 = f3;
0647 x1 = x3;
0648 f2 = fx;
0649 x2 = x0;
0650 x0 = xx;
0651 fx = ff;
0652 goto L3;
0653 }
0654
0655 rv = ee;
0656 ff = func(x0);
0657 return 0;
0658 }
0659
0660 }