void ScanSector(SectorScanData* sectorScanData, int rowNumber, float leftMaxSlope, float rightMinSlope) {
LOGD("new sector scan from row #%d, slope %f -> %f", rowNumber, leftMaxSlope, rightMinSlope);
if (leftMaxSlope < rightMinSlope) {
LOGD("left slope < right slope; abort current scan");
return;
}
FieldOfViewData* fieldOfViewData = sectorScanData->fieldOfViewData;
float nextRowLeftMaxSlope = leftMaxSlope;
bool previousCellIsSolid = false;
for (int currentRowNumber = rowNumber; currentRowNumber <= fieldOfViewData->viewDistance && !previousCellIsSolid; ++currentRowNumber) {
ZPositionDiff rowFirstCellDiff = kLeftEdgeDiff * currentRowNumber;
ZPositionDiff rowCellAfterLastDiff = kRightEdgeDiff * currentRowNumber + kRowDiff;
float currentRowLeftMaxSlope = nextRowLeftMaxSlope;
LOGD("scan row #%d, slope %f -> %f", currentRowNumber, currentRowLeftMaxSlope, rightMinSlope);
for (ZPositionDiff cellDiff = rowFirstCellDiff; cellDiff != rowCellAfterLastDiff; cellDiff += kRowDiff) {
float cellRightSlope = CalcSlope(cellDiff, CellSlope::Right);
if (cellRightSlope > currentRowLeftMaxSlope) {
continue;
}
float cellLeftSlope = CalcSlope(cellDiff, CellSlope::Left);
if (cellLeftSlope < rightMinSlope) {
break;
}
if (fieldOfViewData->CellIsIsTooFar(cellDiff)) {
continue;
}
LOGD("cell %s with slopes [%f; %f] is visible", cellDiff.ToString().c_str(), cellLeftSlope, cellRightSlope);
ZPositionDiff realSectorCellDiff = sectorScanData->conversionMatrix * cellDiff;
fieldOfViewData->MarkCellAsVisible(realSectorCellDiff);
bool cellIsSolid = fieldOfViewData->CellIsSolid(realSectorCellDiff);
if (cellIsSolid) {
nextRowLeftMaxSlope = cellRightSlope;
if (previousCellIsSolid) {
continue;
} else {
LOGD("current cell is first solid; initiating subscan");
ScanSector(sectorScanData, currentRowNumber + 1, currentRowLeftMaxSlope, cellLeftSlope);
}
} else if (previousCellIsSolid) {
currentRowLeftMaxSlope = nextRowLeftMaxSlope;
}
previousCellIsSolid = cellIsSolid;
}
}
}
double GuideAlgorithmLowpass::result(double input)
{
m_history.Add(input);
ArrayOfDbl sortedHistory(m_history);
sortedHistory.Sort(dbl_sort_func);
m_history.RemoveAt(0);
unsigned int numpts = m_history.GetCount();
double median = sortedHistory[sortedHistory.GetCount()/2];
double slope = CalcSlope(m_history);
double dReturn = median + m_slopeWeight*slope;
if (fabs(dReturn) > fabs(input))
{
Debug.Write(wxString::Format("GuideAlgorithmLowpass::Result() input %.2f is < calculated value %.2f, using input\n", input, dReturn));
dReturn = input;
}
//TODO: Undertand this. I think this is wrong, since it divides the orignial input
// by 11 if it was less than the computed value. And since the computed
// value is median + slope, I'm not sure that it should be divided by 11
// either. But the goal of this exercise is to be bug for bug compatible
// with PHD 1.x
if (fabs(input) < m_minMove)
{
dReturn = 0.0;
}
Debug.Write(wxString::Format("GuideAlgorithmLowpass::Result() returns %.2f from input %.2f\n", dReturn, input));
return dReturn;
}
