VVIObjF.cc

Go to the documentation of this file.

//
//  VVIObjF.cc  Version 2.0
//
//  Port of CERNLIB G116 Functions vviden/vvidis
//
// Created by Morris Swartz on 1/14/2010.
// 2010 __TheJohnsHopkinsUniversity__.
//
// V1.1 - make dzero call both fcns with a switch
// V1.2 - remove inappriate initializers and add methods to return non-zero/normalized region
// V2.0 - restructuring and speed improvements by V. Innocente
// V3.0 - further simplification and speedup by Tamas Vami
//

#ifndef SI_PIXEL_TEMPLATE_STANDALONE
// put CMSSW location of SimpleHelix.h here
#include "RecoLocalTracker/SiPixelRecHits/interface/VVIObjF.h"
#else
#include "VVIObjF.h"
#endif

#include <cmath>
#include <algorithm>
#include "vdt/vdtMath.h"

namespace VVIObjFDetails {
  void sincosint(float x, float& sint, float& cint);  
  float expint(float x);                              

  template <typename F>
  int dzero(float a, float b, float& x0, float& rv, float eps, int mxf, F func);
}  // namespace VVIObjFDetails

// ***************************************************************************************************************************************
// ***************************************************************************************************************************************

// WARNING: if you change this, dont forget to change VVIObjF::VVIObjF(float kappa) too
VVIObjF::VVIObjF(float kappa, float beta2, int mode) : mode_(mode) {
  const float xp[9] = {9.29, 2.47, .89, .36, .15, .07, .03, .02, 0.0};
  const float xq[7] = {.012, .03, .08, .26, .87, 3.83, 11.0};
  float h_[7];
  float q, u, h4, h5, h6, q2, d, ll, ul, rv;
  int lp, lq, k, l, n;

  // Make sure that the inputs are reasonable

  if (kappa < 0.01f)
    kappa = 0.01f;
  if (kappa > 10.f)
    kappa = 10.f;
  if (beta2 < 0.f)
    beta2 = 0.f;
  if (beta2 > 1.f)
    beta2 = 1.f;

  float invKappa = 1.f / kappa;
  h_[4] = 1.f - beta2 * 0.42278433999999998f + (7.6f * invKappa);
  h_[5] = beta2;
  h_[6] = 1.f - beta2;
  h4 = -(7.6f * invKappa) - (beta2 * .57721566f + 1.f);
  h5 = vdt::fast_logf(kappa);
  h6 = invKappa;
  t0_ = (h4 - h_[4] * h5 - (h_[4] + beta2) * (vdt::fast_logf(h_[4]) + VVIObjFDetails::expint(h_[4])) +
         vdt::fast_expf(-h_[4])) /
        h_[4];

  // Set up limits for the root search

  for (lp = 0; lp < 9; ++lp) {
    if (kappa >= xp[lp])
      break;
  }
  ll = -float(lp) - 1.5f;
  for (lq = 0; lq < 7; ++lq) {
    if (kappa <= xq[lq])
      break;
  }
  ul = lq - 6.5f;
  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);
  };
  VVIObjFDetails::dzero(ll, ul, u, rv, 1.e-3f, 100, f2);
  q = 1. / u;
  t1_ = h4 * q - h5 - (beta2 * q + 1.f) * (vdt::fast_logf((fabs(u))) + VVIObjFDetails::expint(u)) +
        vdt::fast_expf(-u) * q;
  t_ = t1_ - t0_;
  omega_ = 6.2831853000000004f / t_;
  h_[0] = kappa * (beta2 * .57721566f + 2.f) + 9.9166128600000008f;
  if (kappa >= .07) {
    h_[0] += 6.90775527f;
  }
  h_[1] = beta2 * kappa;
  h_[2] = h6 * omega_;
  h_[3] = omega_ * 1.5707963250000001f;
  auto f1 = [h_](float x) { return h_[0] + h_[1] * vdt::fast_logf(h_[2] * x) - h_[3] * x; };
  VVIObjFDetails::dzero(5.f, 155.f, x0_, rv, 1.e-3f, 100, f1);
  n = x0_ + 1.;
  d = vdt::fast_expf(kappa * (beta2 * (.57721566f - h5) + 1.f)) * .31830988654751274f;
  a_[n - 1] = 0.f;
  if (mode_ == 0) {
    a_[n - 1] = omega_ * .31830988654751274f;
  }
  q = -1.;
  q2 = 2.;

  float x[n];
  x[0] = 0.f;
  float x1[n];
  x1[0] = 0.f;
  float c1[n];
  c1[0] = 0.f;
  float c2[n];
  c2[0] = 0.f;
  float c3[n];
  c3[0] = 0.f;
  float c4[n];
  c4[0] = 0.f;
  float s[n];
  s[0] = 0.f;
  float c[n];
  c[0] = 0.f;
  float xf1[n];
  xf1[0] = 0.f;
  float xf2[n];
  xf2[0] = 0.f;
  for (k = 1; k < n; ++k) {
    x[k] = omega_ * k;
    x1[k] = h6 * x[k];
  }
  for (k = 1; k < n; ++k) {
    VVIObjFDetails::sincosint(x1[k], c2[k], c1[k]);
  }
  for (k = 1; k < n; ++k) {
    c1[k] = vdt::fast_logf(x[k]) - c1[k];
  }
  for (k = 1; k < n; ++k) {
    vdt::fast_sincosf(x1[k], c3[k], c4[k]);
    xf1[k] = kappa * (beta2 * c1[k] - c4[k]) - x[k] * c2[k];
    xf2[k] = x[k] * c1[k] + kappa * (c3[k] + beta2 * c2[k]) + t0_ * x[k];
  }
  for (k = 1; k < n; ++k) {
    vdt::fast_sincosf(xf2[k], s[k], c[k]);
  }
  float d1[n];
  d1[0] = 0.f;
  if (mode_ == 0) {
    for (k = 1; k < n; ++k) {
      d1[k] = q * d * omega_ * vdt::fast_expf(xf1[k]);
      q = -q;
    }
    for (k = 1; k < n; ++k) {
      l = n - k;
      a_[l - 1] = d1[k] * c[k];
      b_[l - 1] = -d1[k] * s[k];
    }
  } else {
    for (k = 1; k < n; ++k) {
      d1[k] = q * d * vdt::fast_expf(xf1[k]) / k;
      q = -q;
    }
    for (k = 1; k < n; ++k) {
      l = n - k;
      a_[l - 1] = d1[k] * s[k];
    }
    for (k = 1; k < n; ++k) {
      l = n - k;
      b_[l - 1] = d1[k] * c[k];
    }
    for (k = 1; k < n; ++k) {
      l = n - k;
      a_[n - 1] += q2 * a_[l - 1];
      q2 = -q2;
    }
  }
}  // VVIObjF

// ***************************************************************************************************************************************
// ***************************************************************************************************************************************

// WARNING: if you change this, dont forget to change the full constructor too
VVIObjF::VVIObjF(float kappa) : mode_(1) {
  const float xp[9] = {9.29, 2.47, .89, .36, .15, .07, .03, .02, 0.0};
  const float xq[7] = {.012, .03, .08, .26, .87, 3.83, 11.0};
  float h_[5];
  float q, u, h4, h5, h6, q2, d, ll, ul, rv;
  int lp, lq, k, l, n;

  // Make sure that the inputs are reasonable

  if (kappa < 0.01f)
    kappa = 0.01f;
  if (kappa > 10.f)
    kappa = 10.f;

  float invKappa = 1.f / kappa;
  h_[4] = 0.57721566f + (7.6f * invKappa);
  h4 = -(7.6f * invKappa) - 1.57721566f;
  h5 = vdt::fast_logf(kappa);
  h6 = invKappa;
  t0_ = (h4 - h_[4] * h5 - (h_[4] + 1.f) * (vdt::fast_logf(h_[4]) + VVIObjFDetails::expint(h_[4])) +
         vdt::fast_expf(-h_[4])) /
        h_[4];

  // Set up limits for the root search

  for (lp = 0; lp < 9; ++lp) {
    if (kappa >= xp[lp])
      break;
  }
  ll = -float(lp) - 1.5f;
  for (lq = 0; lq < 7; ++lq) {
    if (kappa <= xq[lq])
      break;
  }
  ul = lq - 6.5f;
  auto f2 = [h_](float x) { return h_[4] - x + (vdt::fast_logf(std::abs(x)) + VVIObjFDetails::expint(x)); };
  VVIObjFDetails::dzero(ll, ul, u, rv, 1.e-3f, 100, f2);
  q = 1. / u;
  t1_ = h4 * q - h5 - (q + 1.f) * (vdt::fast_logf((fabs(u))) + VVIObjFDetails::expint(u)) + vdt::fast_expf(-u) * q;
  t_ = t1_ - t0_;
  omega_ = 6.2831853000000004f / t_;
  h_[0] = kappa * 2.57721566f + 9.9166128600000008f;
  if (kappa >= .07) {
    h_[0] += 6.90775527f;
  }
  h_[1] = kappa;
  h_[2] = h6 * omega_;
  h_[3] = omega_ * 1.5707963250000001f;
  auto f1 = [h_](float x) { return h_[0] + h_[1] * vdt::fast_logf(h_[2] * x) - h_[3] * x; };
  VVIObjFDetails::dzero(5.f, 155.f, x0_, rv, 1.e-3f, 100, f1);
  n = x0_ + 1.;
  d = vdt::fast_expf(kappa * ((0.57721566f - h5) + 1.f)) * .31830988654751274f;
  a_[n - 1] = 0.f;
  q = -1.;
  q2 = 2.;

  float x[n];
  x[0] = 0.f;
  float x1[n];
  x1[0] = 0.f;
  float c1[n];
  c1[0] = 0.f;
  float c2[n];
  c2[0] = 0.f;
  float c3[n];
  c3[0] = 0.f;
  float c4[n];
  c4[0] = 0.f;
  float s[n];
  s[0] = 0.f;
  float c[n];
  c[0] = 0.f;
  float xf1[n];
  xf1[0] = 0.f;
  float xf2[n];
  xf2[0] = 0.f;
  for (k = 1; k < n; ++k) {
    x[k] = omega_ * k;
    x1[k] = h6 * x[k];
  }
  for (k = 1; k < n; ++k) {
    VVIObjFDetails::sincosint(x1[k], c2[k], c1[k]);
  }
  for (k = 1; k < n; ++k) {
    c1[k] = vdt::fast_logf(x[k]) - c1[k];
  }
  for (k = 1; k < n; ++k) {
    vdt::fast_sincosf(x1[k], c3[k], c4[k]);
    xf1[k] = kappa * (c1[k] - c4[k]) - x[k] * c2[k];
    xf2[k] = x[k] * c1[k] + kappa * (c3[k] + c2[k]) + t0_ * x[k];
  }
  for (k = 1; k < n; ++k) {
    vdt::fast_sincosf(xf2[k], s[k], c[k]);
  }
  float d1[n];
  d1[0] = 0.f;
  for (k = 1; k < n; ++k) {
    d1[k] = q * d * vdt::fast_expf(xf1[k]) / k;
    q = -q;
  }
  for (k = 1; k < n; ++k) {
    l = n - k;
    a_[l - 1] = d1[k] * s[k];
  }
  for (k = 1; k < n; ++k) {
    l = n - k;
    b_[l - 1] = d1[k] * c[k];
  }
  for (k = 1; k < n; ++k) {
    l = n - k;
    a_[n - 1] += q2 * a_[l - 1];
    q2 = -q2;
  }

}  // VVIObjF with kappa only

// *************************************************************************************************************************************
// *************************************************************************************************************************************

float VVIObjF::fcn(float x) const {
  // Local variables

  float f, u, y, a0, a1;
  float a2 = 0.;
  float b1, b0, b2, cof;
  int k, n, n1;

  n = x0_;
  if (x < t0_) {
    f = 0.f;
  } else if (x <= t1_) {
    y = x - t0_;
    u = omega_ * y - 3.141592653589793f;
    float su, cu;
    vdt::fast_sincosf(u, su, cu);
    cof = cu * 2.f;
    a1 = 0.;
    a0 = a_[0];
    n1 = n + 1;
    for (k = 2; k <= n1; ++k) {
      a2 = a1;
      a1 = a0;
      a0 = a_[k - 1] + cof * a1 - a2;
    }
    b1 = 0.;
    b0 = b_[0];
    for (k = 2; k <= n; ++k) {
      b2 = b1;
      b1 = b0;
      b0 = b_[k - 1] + cof * b1 - b2;
    }
    f = (a0 - a2) * .5f + b0 * su;
    if (mode_ != 0) {
      f += y / t_;
    }
  } else {
    f = 0.f;
    if (mode_ != 0) {
      f = 1.f;
    }
  }
  return f;
}  // fcn

// *************************************************************************************************************************************
// *************************************************************************************************************************************

void VVIObjF::limits(float& xl, float& xu) const {
  xl = t0_;
  xu = t1_;
  return;
}  // limits

#include "sicif.h"
namespace VVIObjFDetails {
  void sincosint(float x, float& sint, float& cint) { sicif(x, sint, cint); }

  float expint(float x) {
    // Initialized data

    const float zero = 0.;
    const float q2[7] = {
        .10340013040487, 3.319092135933, 20.449478501379, 41.280784189142, 32.426421069514, 10.041164382905, 1.};
    const float p3[6] = {
        -2.3909964453136, -147.98219500504, -254.3763397689, -119.55761038372, -19.630408535939, -.9999999999036};
    const float q3[6] = {177.60070940351, 530.68509610812, 462.23027156148, 156.81843364539, 21.630408494238, 1.};
    const float p4[8] = {-8.6693733995107,
                         -549.14226552109,
                         -4210.0161535707,
                         -249301.39345865,
                         -119623.66934925,
                         -22174462.775885,
                         3892804.213112,
                         -391546073.8091};
    const float q4[8] = {34.171875,
                         -1607.0892658722,
                         35730.029805851,
                         -483547.43616216,
                         4285596.2461175,
                         -24903337.574054,
                         89192576.757561,
                         -165254299.72521};
    const float a1[8] = {-2.1808638152072,
                         -21.901023385488,
                         9.3081638566217,
                         25.076281129356,
                         -33.184253199722,
                         60.121799083008,
                         -43.253113287813,
                         1.0044310922808};
    const float b1[8] = {0.,
                         3.9370770185272,
                         300.89264837292,
                         -6.2504116167188,
                         1003.6743951673,
                         14.325673812194,
                         2736.2411988933,
                         .52746885196291};
    const float a2[8] = {-3.4833465360285,
                         -18.65454548834,
                         -8.2856199414064,
                         -32.34673303054,
                         17.960168876925,
                         1.7565631546961,
                         -1.9502232128966,
                         .99999429607471};
    const float b2[8] = {0.,
                         69.500065588743,
                         57.283719383732,
                         25.777638423844,
                         760.76114800773,
                         28.951672792514,
                         -3.4394226689987,
                         1.0008386740264};
    const float a3[6] = {
        -27.780928934438, -10.10479081576, -9.1483008216736, -5.0223317461851, -3.0000077799358, 1.0000000000704};
    const float one = 1.;
    const float b3[6] = {0., 122.39993926823, 2.7276100778779, -7.1897518395045, -2.9990118065262, 1.999999942826};
    const float two = 2.;
    const float three = 3.;
    const float x0 = .37250741078137;
    const float xl[6] = {-24., -12., -6., 0., 1., 4.};
    const float p1[5] = {4.293125234321, 39.894153870321, 292.52518866921, 425.69682638592, -434.98143832952};
    const float q1[5] = {1., 18.899288395003, 150.95038744251, 568.05252718987, 753.58564359843};
    const float p2[7] = {.43096783946939,
                         6.9052252278444,
                         23.019255939133,
                         24.378408879132,
                         9.0416155694633,
                         .99997957705159,
                         4.656271079751e-7};

    // Local variables
    float v, y, ap, bp, aq, dp, bq, dq;

    if (x <= xl[0]) {
      ap = a3[0] - x;
      for (int i__ = 2; i__ <= 5; ++i__) {
        ap = a3[i__ - 1] - x + b3[i__ - 1] / ap;
      }
      y = vdt::fast_expf(-x) / x * (one - (a3[5] + b3[5] / ap) / x);
    } else if (x <= xl[1]) {
      ap = a2[0] - x;
      for (int i__ = 2; i__ <= 7; ++i__) {
        ap = a2[i__ - 1] - x + b2[i__ - 1] / ap;
      }
      y = vdt::fast_expf(-x) / x * (a2[7] + b2[7] / ap);
    } else if (x <= xl[2]) {
      ap = a1[0] - x;
      for (int i__ = 2; i__ <= 7; ++i__) {
        ap = a1[i__ - 1] - x + b1[i__ - 1] / ap;
      }
      y = vdt::fast_expf(-x) / x * (a1[7] + b1[7] / ap);
    } else if (x < xl[3]) {
      v = -two * (x / three + one);
      bp = zero;
      dp = p4[0];
      for (int i__ = 2; i__ <= 8; ++i__) {
        ap = bp;
        bp = dp;
        dp = p4[i__ - 1] - ap + v * bp;
      }
      bq = zero;
      dq = q4[0];
      for (int i__ = 2; i__ <= 8; ++i__) {
        aq = bq;
        bq = dq;
        dq = q4[i__ - 1] - aq + v * bq;
      }
      y = -vdt::fast_logf(-x / x0) + (x + x0) * (dp - ap) / (dq - aq);
    } else if (x == xl[3]) {
      return zero;
    } else if (x < xl[4]) {
      ap = p1[0];
      aq = q1[0];
      for (int i__ = 2; i__ <= 5; ++i__) {
        ap = p1[i__ - 1] + x * ap;
        aq = q1[i__ - 1] + x * aq;
      }
      y = -vdt::fast_logf(x) + ap / aq;
    } else if (x <= xl[5]) {
      y = one / x;
      ap = p2[0];
      aq = q2[0];
      for (int i__ = 2; i__ <= 7; ++i__) {
        ap = p2[i__ - 1] + y * ap;
        aq = q2[i__ - 1] + y * aq;
      }
      y = vdt::fast_expf(-x) * ap / aq;
    } else {
      y = one / x;
      ap = p3[0];
      aq = q3[0];
      for (int i__ = 2; i__ <= 6; ++i__) {
        ap = p3[i__ - 1] + y * ap;
        aq = q3[i__ - 1] + y * aq;
      }
      y = vdt::fast_expf(-x) * y * (one + y * ap / aq);
    }
    return y;
  }  // expint

  template <typename F>
  int dzero(float a, float b, float& x0, float& rv, float eps, int mxf, F func) {
    /* System generated locals */
    float d__1, d__2, d__3, d__4;

    // Local variables
    float f1, f2, f3, u1, u2, x1, x2, u3, u4, x3, ca, cb, cc, fa, fb, ee, ff;
    int mc;
    float xa, xb, fx, xx, su4;

    xa = std::min(a, b);
    xb = std::max(a, b);
    fa = func(xa);
    fb = func(xb);
    if (fa * fb > 0.f) {
      rv = (xb - xa) * -2.f;
      x0 = 0.f;
      return 1;
    }
    mc = 0;
  L1:
    x0 = (xa + xb) * 0.5f;
    rv = x0 - xa;
    ee = eps * (std::abs(x0) + 1.f);
    if (rv <= ee) {
      rv = ee;
      ff = func(x0);
      return 0;
    }
    f1 = fa;
    x1 = xa;
    f2 = fb;
    x2 = xb;
  L2:
    fx = func(x0);
    ++mc;
    if (mc > mxf) {
      rv = (d__1 = xb - xa, fabs(d__1)) * -0.5f;
      x0 = 0.;
      return 0;
    }
    if (fx * fa > 0.f) {
      xa = x0;
      fa = fx;
    } else {
      xb = x0;
      fb = fx;
    }
  L3:
    u1 = f1 - f2;
    u2 = x1 - x2;
    u3 = f2 - fx;
    u4 = x2 - x0;
    if (u2 == 0.f || u4 == 0.f) {
      goto L1;
    }
    f3 = fx;
    x3 = x0;
    u1 /= u2;
    u2 = u3 / u4;
    ca = u1 - u2;
    cb = (x1 + x2) * u2 - (x2 + x0) * u1;
    cc = (x1 - x0) * f1 - x1 * (ca * x1 + cb);
    if (ca == 0.f) {
      if (cb == 0.f) {
        goto L1;
      }
      x0 = -cc / cb;
    } else {
      u3 = cb / (ca * 2.f);
      u4 = u3 * u3 - cc / ca;
      if (u4 < 0.f) {
        goto L1;
      }
      su4 = std::abs(u4);
      if (x0 + u3 < 0.f) {
        su4 = -su4;
      }
      x0 = -u3 + su4;
    }
    if (x0 < xa || x0 > xb) {
      goto L1;
    }
    // Computing MIN
    d__3 = (d__1 = x0 - x3, std::abs(d__1));
    d__4 = (d__2 = x0 - x2, std::abs(d__2));
    rv = std::min(d__3, d__4);
    ee = eps * (std::abs(x0) + 1);
    if (rv > ee) {
      f1 = f2;
      x1 = x2;
      f2 = f3;
      x2 = x3;
      goto L2;
    }
    fx = func(x0);
    if (fx == 0.f) {
      rv = ee;
      ff = func(x0);
      return 0;
    }
    if (fx * fa < 0.f) {
      xx = x0 - ee;
      if (xx <= xa) {
        rv = ee;
        ff = func(x0);
        return 0;
      }
      ff = func(xx);
      fb = ff;
      xb = xx;
    } else {
      xx = x0 + ee;
      if (xx >= xb) {
        rv = ee;
        ff = func(x0);
        return 0;
      }
      ff = func(xx);
      fa = ff;
      xa = xx;
    }
    if (fx * ff > 0.f) {
      mc += 2;
      if (mc > mxf) {
        rv = (d__1 = xb - xa, std::abs(d__1)) * -0.5f;
        x0 = 0.f;
        return 0;
      }
      f1 = f3;
      x1 = x3;
      f2 = fx;
      x2 = x0;
      x0 = xx;
      fx = ff;
      goto L3;
    }
    /* L4: */
    rv = ee;
    ff = func(x0);
    return 0;
  }  // dzero

}  // namespace VVIObjFDetails