src/firmware/Cordic.cc

0001 #include "L1Trigger/L1TCalorimeter/interface/Cordic.h"
0002
0003 #include <cstdint>
0004 #include <cmath>
0005 #include <vector>
0006 #include <iostream>
0007 #include <iomanip>
0008
0009 Cordic::Cordic(const uint32_t& aPhiScale, const uint32_t& aMagnitudeBits, const uint32_t& aSteps)
0010     : mPhiScale(aPhiScale),
0011       mMagnitudeScale(1 << aMagnitudeBits),
0012       mMagnitudeBits(aMagnitudeBits),
0013       mSteps(aSteps),
0014       mPi(3.1415926535897932384626433832795) {
0015   mRotations.reserve(mSteps);
0016
0017   double lValue(1.0);
0018
0019   for (uint32_t lStep(0); lStep != mSteps; ++lStep) {
0020     lValue /= sqrt(1.0 + pow(4.0, -double(lStep)));
0021     mRotations.push_back(tower(atan(pow(2.0, -double(lStep)))));
0022   }
0023   mMagnitudeRenormalization = uint32_t(round(mMagnitudeScale * lValue));
0024 }
0025
0026 Cordic::~Cordic() {}
0027
0028 double Cordic::NormalizePhi(const uint32_t& aPhi) const { return double(aPhi) / double(mPhiScale); }
0029
0030 double Cordic::NormalizeMagnitude(const uint32_t& aMagnitude) const {
0031   return double(aMagnitude) / double(mMagnitudeScale);
0032 }
0033
0034 int32_t Cordic::IntegerizeMagnitude(const double& aMagnitude) const { return int32_t(aMagnitude * mMagnitudeScale); }
0035
0036 uint32_t Cordic::tower(const double& aRadians) const { return uint32_t(round(mPhiScale * 0.5 * aRadians / mPi)); }
0037
0038 void Cordic::operator()(int32_t aX, int32_t aY, int32_t& aPhi, uint32_t& aMagnitude) const {
0039   bool lSign(true);
0040
0041   switch (((aY >= 0) ? 0x0 : 0x2) | ((aX >= 0) ? 0x0 : 0x1)) {
0042     case 0:
0043       aPhi = tower(mPi);
0044       break;
0045     case 1:
0046       aPhi = tower(2 * mPi);
0047       lSign = false;
0048       aX = -aX;
0049       break;
0050     case 2:
0051       aPhi = tower(mPi);
0052       lSign = false;
0053       aY = -aY;
0054       break;
0055     case 3:
0056       aPhi = 0;
0057       aX = -aX;
0058       aY = -aY;
0059       break;
0060     default:
0061       throw 0;
0062   }
0063
0064   for (uint32_t lStep(0); lStep != mSteps; ++lStep) {
0065     if ((aY < 0) == lSign) {
0066       aPhi -= mRotations[lStep];
0067     } else {
0068       aPhi += mRotations[lStep];
0069     }
0070
0071     int32_t lX(aX), lY(aY);
0072     if (lY < 0) {
0073       aX = lX - (lY >> lStep);
0074       aY = lY + (lX >> lStep);
0075     } else {
0076       aX = lX + (lY >> lStep);
0077       aY = lY - (lX >> lStep);
0078     }
0079   }
0080
0081   aMagnitude = (aX * mMagnitudeRenormalization) >> mMagnitudeBits;
0082 }