56 const std::array<DiscretePhi0, 2>& phi0Vec = houghBox.getBounds<DiscretePhi0>();
57 const std::array<DiscreteCurv, 2>& curv = houghBox.getBounds<DiscreteCurv>();
59 std::array<float, 2> xRot;
60 xRot[0] = x * phi0Vec[0]->x() + y * phi0Vec[0]->y();
61 xRot[1] = x * phi0Vec[1]->x() + y * phi0Vec[1]->y();
63 const bool isNonCurler = (
static_cast<float>(curv[1]) <=
m_curlCurv and
65 const bool onlyPositiveArm = isNonCurler;
67 if (onlyPositiveArm) {
69 if (xRot[0] < 0 and xRot[1] < 0)
return ESign::c_Invalid;
72 std::array<float, 2> yRot;
73 yRot[0] = -x * phi0Vec[0]->y() + y * phi0Vec[0]->x();
74 yRot[1] = -x * phi0Vec[1]->y() + y * phi0Vec[1]->x();
76 const std::array<ContinuousImpact, 2>& impact = houghBox.getBounds<ContinuousImpact>();
79 std::array<float, 4> yRotMinusI;
80 yRotMinusI[0b00] = yRot[0] - impact[0];
81 yRotMinusI[0b01] = yRot[0] - impact[1];
82 yRotMinusI[0b10] = yRot[1] - impact[0];
83 yRotMinusI[0b11] = yRot[1] - impact[1];
85 const float l2 = l * l;
86 std::array<float, 4> r2MinusI;
87 r2MinusI[0b00] = xRot[0] * xRot[0] + yRotMinusI[0b00] * yRotMinusI[0b00] - l2;
88 r2MinusI[0b01] = xRot[0] * xRot[0] + yRotMinusI[0b01] * yRotMinusI[0b01] - l2;
89 r2MinusI[0b10] = xRot[1] * xRot[1] + yRotMinusI[0b10] * yRotMinusI[0b10] - l2;
90 r2MinusI[0b11] = xRot[1] * xRot[1] + yRotMinusI[0b11] * yRotMinusI[0b11] - l2;
94 std::array<ESign, 2> distSign;
95 for (
int c_Curv = 0; c_Curv < 2; ++c_Curv) {
96 std::array<float, 4> dist;
97 float curvHalf =
static_cast<float>(curv[c_Curv]) / 2;
98 dist[0b00] = - yRotMinusI[0b00] + r2MinusI[0b00] * curvHalf - l;
99 dist[0b01] = - yRotMinusI[0b01] + r2MinusI[0b01] * curvHalf - l;
100 dist[0b10] = - yRotMinusI[0b10] + r2MinusI[0b10] * curvHalf - l;
101 dist[0b11] = - yRotMinusI[0b11] + r2MinusI[0b11] * curvHalf - l;