L1TTrackMatch/src/Cordic.cc

0001 #include "L1Trigger/L1TTrackMatch/interface/Cordic.h"
0002
0003 #include <cmath>
0004 #include <iomanip>
0005 #include <memory>
0006
0007 using namespace l1tmetemu;
0008
0009 Cordic::Cordic(const int aSteps, bool debug) : cordicSteps(aSteps), debug(debug) {
0010   atanLUT.reserve(aSteps);
0011   magNormalisationLUT.reserve(aSteps);
0012
0013   if (debug) {
0014     edm::LogVerbatim("L1TkEtMissEmulator") << "=====atan LUT=====";
0015   }
0016
0017   for (int i = 0; i < aSteps; i++) {
0018     atanLUT.push_back(l1tmetemu::atan_lut_fixed_t(atan(pow(2, -i))));
0019     if (debug) {
0020       edm::LogVerbatim("L1TkEtMissEmulator") << atanLUT[i] << " | ";
0021     }
0022   }
0023   if (debug) {
0024     edm::LogVerbatim("L1TkEtMissEmulator") << "\n=====Normalisation LUT=====";
0025   }
0026
0027   double val = 1.0;
0028   for (int j = 0; j < aSteps; j++) {
0029     val = val / (pow(1 + pow(4, -j), 0.5));
0030     magNormalisationLUT.push_back(l1tmetemu::atan_lut_fixed_t(val));
0031     if (debug) {
0032       edm::LogVerbatim("L1TkEtMissEmulator") << magNormalisationLUT[j] << " | ";
0033     }
0034   }
0035 }
0036
0037 template <typename T>
0038 void Cordic::cordic_subfunc(T &x, T &y, T &z) const {
0039   if (debug) {
0040     edm::LogVerbatim("L1TkEtMissEmulator") << "\n=====Cordic Initial Conditions=====\n"
0041                                            << "Cordic x: " << x << " Cordic y: " << y << " Cordic z: " << z << "\n"
0042                                            << "\n=====Cordic Steps=====";
0043   }
0044
0045   T tx, ty, tz;
0046
0047   for (int step = 0; step < cordicSteps; step++) {
0048     if (y < 0) {
0049       tx = x - (y >> step);
0050       ty = y + (x >> step);
0051       tz = z - atanLUT[step];
0052     } else {
0053       tx = x + (y >> step);
0054       ty = y - (x >> step);
0055       tz = z + atanLUT[step];
0056     }
0057
0058     x = tx;
0059     y = ty;
0060     z = tz;
0061
0062     if (debug) {
0063       edm::LogVerbatim("L1TkEtMissEmulator")
0064           << "Cordic x: " << x << " Cordic y: " << y << " Cordic phi: " << z << "\n"
0065           << " Cordic gain: " << magNormalisationLUT[step] << " kStepMETwordPhi: " << kStepMETwordPhi << "\n";
0066     }
0067   }
0068 }
0069
0070 EtMiss Cordic::toPolar(Et_t x, Et_t y) const {
0071   EtMiss ret_etmiss;
0072
0073   if (debug) {
0074     edm::LogVerbatim("L1TkEtMissEmulator") << "\n=====toPolar input=====\n"
0075                                            << "x: " << x << " y: " << y;
0076   }
0077
0078   // Some needed constants
0079   const ap_fixed<l1tmetemu::Et_t::width + 1, 3> pi = M_PI;                   // pi
0080   const ap_fixed<l1tmetemu::Et_t::width + 2, 3> pi2 = M_PI / 2.;             // pi/2
0081   const l1tmetemu::METWordphi_t pistep = M_PI / l1tmetemu::kStepMETwordPhi;  // (pi) / l1tmetemu::kStepMETwordPhi
0082   const l1tmetemu::METWordphi_t pi2step = pistep / 2.;                       // (pi/2) / l1tmetemu::kStepMETwordPhi
0083
0084   // Find the sign of the inputs
0085   ap_uint<2> signx = (x > 0) ? 2 : (x == 0) ? 1 : 0;
0086   ap_uint<2> signy = (y > 0) ? 2 : (y == 0) ? 1 : 0;
0087
0088   // Corner cases
0089   if (signy == 1 && signx == 2) {  // y == 0 and x > 0
0090     ret_etmiss.Et = x;
0091     ret_etmiss.Phi = 0;
0092     return ret_etmiss;
0093   } else if (signy == 1 && signx == 0) {  // y == 0 and x < 0
0094     ret_etmiss.Et = -x;
0095     ret_etmiss.Phi = pistep;
0096     return ret_etmiss;
0097   } else if (signy == 2 && signx == 1) {  // y > 0 and x == 0
0098     ret_etmiss.Et = y;
0099     ret_etmiss.Phi = pi2step;
0100     return ret_etmiss;
0101   } else if (signy == 0 && signx == 1) {  // y < 0 and x == 0
0102     ret_etmiss.Et = -y;
0103     ret_etmiss.Phi = -pi2step;
0104     return ret_etmiss;
0105   }
0106
0107   // Take absolute values to operate on the range (0, pi/2)
0108   ap_fixed<Et_t::width + 1, Et_t::iwidth + 1, Et_t::qmode, Et_t::omode> absx, absy;
0109   if (signy == 0) {
0110     absy = -y;
0111   } else {
0112     absy = y;
0113   }
0114
0115   if (signx == 0) {
0116     absx = -x;
0117   } else {
0118     absx = x;
0119   }
0120
0121   if (debug) {
0122     edm::LogVerbatim("L1TkEtMissEmulator") << "\n=====Abs input=====\n"
0123                                            << "abs(x): " << absx.to_double() << " abs(y): " << absy.to_double();
0124   }
0125
0126   // Normalization (operate on a unit circle)
0127   ap_fixed<Et_t::width + 1, 2, Et_t::qmode, Et_t::omode> absx_sft, absy_sft;
0128   for (int i = 0; i < Et_t::width + 1; i++) {
0129     absx_sft[i] = absx[i];
0130     absy_sft[i] = absy[i];
0131   }
0132
0133   if (debug) {
0134     edm::LogVerbatim("L1TkEtMissEmulator")
0135         << "\n=====Normalized input=====\n"
0136         << "norm(abs(x)): " << absx_sft.to_double() << " norm(abs(y)): " << absy_sft.to_double();
0137   }
0138
0139   // Setup the CORDIC inputs/outputs
0140   ap_fixed<Et_t::width + 7, 3, Et_t::qmode, Et_t::omode> cx, cy, cphi;
0141   if (absy > absx) {
0142     cx = absy_sft;
0143     cy = absx_sft;
0144     cphi = 0;
0145   } else {
0146     cx = absx_sft;
0147     cy = absy_sft;
0148     cphi = 0;
0149   }
0150
0151   if (debug) {
0152     edm::LogVerbatim("L1TkEtMissEmulator")
0153         << "\n=====CORDIC function arguments=====\n"
0154         << "x: " << cx.to_double() << " y: " << cy.to_double() << " phi: " << cphi.to_double();
0155   }
0156
0157   // Perform the CORDIC (vectoring) function
0158   cordic_subfunc(cx, cy, cphi);
0159
0160   // Reorient the outputs to their appropriate quadrant
0161   if (absy > absx) {
0162     cphi = pi2 - cphi;
0163   }
0164
0165   ap_fixed<Et_t::width, 3, Et_t::qmode, Et_t::omode> ophi;
0166   if (signx == 0 && signy == 2) {  // x < 0 and y > 0
0167     ophi = pi - cphi;
0168   } else if (signx == 0 && signy == 0) {  // x < 0 and y < 0
0169     ophi = cphi - pi;
0170   } else if (signx == 2 && signy == 0) {  // x > 0 and y < 0
0171     ophi = -cphi;
0172   } else {
0173     ophi = cphi;
0174   }
0175
0176   // Re-scale the outputs
0177   Et_t magnitude = ((ap_fixed<Et_t::width + Et_t::iwidth + 3 + 1, Et_t::iwidth, Et_t::qmode, Et_t::omode>)cx)
0178                    << (Et_t::iwidth + 1 - 2);
0179   ret_etmiss.Et = l1tmetemu::Et_t(magnitude * magNormalisationLUT[cordicSteps - 1]);
0180   ret_etmiss.Phi = ophi * pi_bins_fixed_t(kBinsInPi);
0181
0182   if (debug) {
0183     edm::LogVerbatim("L1TkEtMissEmulator")
0184         << "\n=====toPolar output=====\n"
0185         << std::setprecision(8) << "magnitude: " << magnitude.to_double() << " phi: " << ophi.to_double()
0186         << " kBinsInPi: " << pi_bins_fixed_t(kBinsInPi).to_double() << "\n"
0187         << "Et: " << ret_etmiss.Et.to_double() << " phi (int): " << ret_etmiss.Phi.to_int() << "\n";
0188   }
0189
0190   return ret_etmiss;
0191 }