std::vector interp()

in sensor_fusion_pkg/src/utility.cpp [69:88]

• 20 lines of code
• 4 McCabe index (conditional complexity)

    std::vector<float> interp(const std::vector<float>& sortedX, const std::vector<float>& sortedXp, const std::vector<float>& sortedFp){
        size_t currXpIdx = 0;
        size_t currXidx = 0;
        std::vector<float> returnArray;
        while (currXidx < sortedX.size())
        {
            if (sortedXp[currXpIdx] <= sortedX[currXidx] && sortedX[currXidx] <= sortedXp[currXpIdx + 1])
            {
                const float percent = static_cast<float>(sortedX[currXidx] - sortedXp[currXpIdx]) /
                    static_cast<float>(sortedXp[currXpIdx + 1] - sortedXp[currXpIdx]);
                returnArray.push_back(sortedFp[currXpIdx] * (1.0 - percent) + sortedFp[currXpIdx + 1] * percent);
                ++currXidx;
            }
            else
            {
                ++currXpIdx;
            }
        }
        return returnArray;
    }