Get interpolated value from 2D matrix

CalcMatrix.cpp

// Mauro Grassia (c) 2022
// The Unlicense
// This is free and unencumbered software released into the public domain.
// 
// Anyone is free to copy, modify, publish, use, compile, sell, or
// distribute this software, either in source code form or as a compiled
// binary, for any purpose, commercial or non-commercial, and by any
// means.
// 
// In jurisdictions that recognize copyright laws, the author or authors
// of this software dedicate any and all copyright interest in the
// software to the public domain. We make this dedication for the benefit
// of the public at large and to the detriment of our heirs and
// successors. We intend this dedication to be an overt act of
// relinquishment in perpetuity of all present and future rights to this
// software under copyright law.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
// IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
// OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
// OTHER DEALINGS IN THE SOFTWARE.
// 
// For more information, please refer to <http://unlicense.org/>

#include <iostream>
#include <cstdint>
#include <map>

template <typename T, std::size_t RowCount, std::size_t ColCount>
T runMatrix(T const (&matrix)[RowCount][ColCount], T rowReference, T colReference)
{
	std::size_t firstColIndex = 0;
	std::size_t secondColIndex = 0;
	
	float fFirstColWeight = 0.f;
	
	// Trovo la prima colonna il cui valore supera colReference
	for(std::size_t iCol = 1; iCol < ColCount; ++iCol) {
		if(matrix[0][iCol] == colReference) {
			firstColIndex = iCol;
			fFirstColWeight = 1.f;
			break;
		}
		else if(matrix[0][iCol] > colReference) {
			if(iCol == 1) {
				firstColIndex = iCol;
				fFirstColWeight = 1.f;
				break;
			}
			else {
				firstColIndex = iCol-1;
				secondColIndex = iCol;
				
				T twoColumnRange = matrix[0][iCol] - matrix[0][iCol-1];
				T posInRange = colReference - matrix[0][iCol-1];
				
				fFirstColWeight = 1.f - (static_cast<float>(posInRange) / static_cast<float>(twoColumnRange));
				break;
			}
		}
	}
	
	// Valore "troppo a destra"
	if(firstColIndex == 0) {
		firstColIndex = ColCount-1;
		fFirstColWeight = 1.f;
	}
	
	// A questo punto mi ritrovo con:
	// firstColIndex > 1 && firstColIndex < ColCount
	// fFirstColWeight > 0.f && fFirstColWeight <= 1.f
	// secondColIndex può essere:
	//  0 = Assente
	//  1 = Presente
	
	std::size_t firstRowIndex = 0;
	std::size_t secondRowIndex = 0;
	
	float fFirstRowWeight = 0.f;
	
	// Adesso trovo la prima riga in cui il valore supera rowReference
	for(std::size_t iRow = 1; iRow < RowCount; ++iRow) {
		if(matrix[iRow][0] == rowReference) {
			firstRowIndex = iRow;
			fFirstRowWeight = 1.f;
			break;
		}
		else if(matrix[iRow][0] > rowReference) {
			if(iRow == 1) {
				firstRowIndex = iRow;
				fFirstRowWeight = 1.f;
				break;
			}
			else {
				firstRowIndex = iRow-1;
				secondRowIndex = iRow;
				
				T twoRowRange = matrix[iRow][0] - matrix[iRow-1][0];
				T posInRange = rowReference - matrix[iRow-1][0];
				
				fFirstRowWeight = 1.f - (static_cast<float>(posInRange) / static_cast<float>(twoRowRange));
				break;
			}
		}
	}
	
	// Valore "troppo in basso"
	if(firstRowIndex == 0) {
		firstRowIndex = RowCount-1;
		fFirstRowWeight = 1.f;
	}
	
	// A questo punto mi ritrovo con:
	// firstRowIndex > 1 && firstRowIndex < RowCount
	// fFirstRowWeight > 0.f && fFirstRowWeight <= 1.f
	// secondRowIndex può essere:
	//  0 = Assente
	//  1 = Presente
	
	// Calculate matrix
	
	float fMatrixResult = (
		  (static_cast<float>(matrix[ firstRowIndex][ firstColIndex]) *        fFirstRowWeight  *        fFirstColWeight)
		+ (static_cast<float>(matrix[secondRowIndex][ firstColIndex]) * (1.f - fFirstRowWeight) *        fFirstColWeight)
		+ (static_cast<float>(matrix[ firstRowIndex][secondColIndex]) *        fFirstRowWeight  * (1.f - fFirstColWeight))
		+ (static_cast<float>(matrix[secondRowIndex][secondColIndex]) * (1.f - fFirstRowWeight) * (1.f - fFirstColWeight))
	);
	
	return static_cast<T>(fMatrixResult);
}

std::int16_t const twoDimMatrix[][15] = {
	{  0,  500, 1000, 1500, 2000, 2500, 3000, 3500, 4000, 4500, 5000, 5500, 6000, 6500, 7000},
	{ 10,    5,   15,   25,   30,   40,   50,   60,   70,   80,   90,  100,  110,  120,  130},
	{ 20,    6,   16,   26,   40,   40,   50,   60,   70,   80,   90,  100,  110,  120,  130},
	{ 30,    7,   17,   27,   50,   30,   50,   60,   70,   80,   90,  100,  110,  120,  130},
	{ 40,    8,   18,   28,   60,   20,   50,   60,   70,   80,   90,  100,  110,  120,  130},
	{ 50,    8,   18,   28,   70,   10,   50,   60,   70,   80,   90,  100,  110,  120,  130},
	{ 60,    8,   18,   28,   80,  -50,   50,   60,   70,   80,   90,  100,  110,  120,  130},
	{ 70,    8,   18,   28,   70,   10,   50,   60,   70,   80,   90,  100,  110,  120,  130},
	{ 80,    8,   18,   28,   60,   20,   50,   60,   70,   80,   90,  100,  110,  120,  130},
	{ 90,    8,   18,   28,   50,   30,   50,   60,   70,   80,   90,  100,  110,  120,  130},
	{100,    8,   18,   28,   40,   40,   50,   60,   70,   80,   90,  100,  110,  120,  130},
};

int main()
{
	std::cout << runMatrix<std::int16_t>(twoDimMatrix, 55, 3250) << std::endl; // 55
	std::cout << runMatrix<std::int16_t>(twoDimMatrix, 63, 2300) << std::endl; // 11
	std::cout << runMatrix<std::int16_t>(twoDimMatrix, 67, 2400) << std::endl; //  8
	std::cout << runMatrix<std::int16_t>(twoDimMatrix, 56, 2000) << std::endl; // 76
	std::cout << runMatrix<std::int16_t>(twoDimMatrix, 56, 3000) << std::endl; // 50
	
	
	return 0;
}