Create an interpolate3d function to match the interpolate2d, but for 3d tables. (#3459)

Remove some implicit C array to pointer conversions in Map3D by adding
(), *, and & where appropriate.  This allows us to remove getBinPtr.

Call interpolate3d from Map3d so there's no code duplication.

firmware/util/containers/table_helper.h

@@ -29,7 +29,8 @@ template<int TColNum, int TRowNum, typename TValue, typename TColumn, typename T
 class Map3D : public ValueProvider3D {
 public:
 	template <typename TValueInit, typename TRowInit, typename TColumnInit>
-	void init(TValueInit table[TRowNum][TColNum], const TRowInit rowBins[TRowNum], const TColumnInit columnBins[TColNum]) {
+	void init(TValueInit (&table)[TRowNum][TColNum],
+                  const TRowInit (&rowBins)[TRowNum], const TColumnInit (&columnBins)[TColNum]) {
 		// This splits out here so that we don't need one overload of init per possible combination of table/rows/columns types/dimensions
 		// Overload resolution figures out the correct versions of the functions below to call, some of which have assertions about what's allowed
 		initValues(table);
@@ -37,73 +38,60 @@ public:
 		initCols(columnBins);
 	}
 
-	float getValue(float xColumn, float yRow) const override {
+	float getValue(float xColumn, float yRow) const final {
 		if (!m_values) {
 			// not initialized, return 0
 			return 0;
 		}
 
-		auto row = priv::getBinPtr<TRow, TRowNum>(yRow * m_rowMult, m_rowBins);
+                return interpolate3d(*m_values,
-		auto col = priv::getBinPtr<TColumn, TColNum>(xColumn * m_colMult, m_columnBins);
+                                     *m_rowBins, yRow * m_rowMult,
-
+                                     *m_columnBins, xColumn * m_colMult) *
-		// Orient the table such that (0, 0) is the bottom left corner,
+                    TValueMultiplier::asFloat();
-		// then the following variable names will make sense
-		float lowerLeft = getValueAtPosition(row.Idx, col.Idx);
-		float upperLeft = getValueAtPosition(row.Idx + 1, col.Idx);
-		float lowerRight = getValueAtPosition(row.Idx, col.Idx + 1);
-		float upperRight = getValueAtPosition(row.Idx + 1, col.Idx + 1);
-
-		// Interpolate each side by itself
-		float left = priv::linterp(lowerLeft, upperLeft, row.Frac);
-		float right = priv::linterp(lowerRight, upperRight, row.Frac);
-
-		// Then interpolate between those
-		float tableValue = priv::linterp(left, right, col.Frac);
-
-		// Correct by the ratio of table units to "world" units
-		return tableValue * TValueMultiplier::asFloat();
 	}
 
 	void setAll(TValue value) {
 		efiAssertVoid(CUSTOM_ERR_6573, m_values, "map not initialized");
 
-		for (size_t i = 0; i < TRowNum * TColNum; i++) {
+                for (size_t r = 0; r < TRowNum; r++) {
-			m_values[i] = value / TValueMultiplier::asFloat();
+                    for (size_t c = 0; c < TColNum; c++) {
+			(*m_values)[r][c] = value / TValueMultiplier::asFloat();
+                    }
 		}
 	}
 
 private:
 	template <int TMult>
-	void initValues(scaled_channel<TValue, TMult> table[TRowNum][TColNum]) {
+	void initValues(scaled_channel<TValue, TMult> (&table)[TRowNum][TColNum]) {
 		static_assert(TValueMultiplier::den == TMult);
 		static_assert(TValueMultiplier::num == 1);
 
-		m_values = reinterpret_cast<TValue*>(&table[0][0]);
+		m_values = reinterpret_cast<TValue (*)[TRowNum][TColNum]>(&table);
 	}
 
-	void initValues(TValue table[TRowNum][TColNum]) {
+    void initValues(TValue (&table)[TRowNum][TColNum]) {
-		m_values = &table[0][0];
+        m_values = &table;
 	}
 
 	template <int TRowMult>
-	void initRows(const scaled_channel<TRow, TRowMult> rowBins[TRowNum]) {
+	void initRows(const scaled_channel<TRow, TRowMult> (&rowBins)[TRowNum]) {
-		m_rowBins = reinterpret_cast<const TRow*>(&rowBins[0]);
+            m_rowBins = reinterpret_cast<const TRow (*)[TRowNum]>(&rowBins);
 		m_rowMult = TRowMult;
 	}
 
-	void initRows(const TRow rowBins[TRowNum]) {
+    void initRows(const TRow (&rowBins)[TRowNum]) {
-		m_rowBins = &rowBins[0];
+		m_rowBins = &rowBins;
 		m_rowMult = 1;
 	}
 
 	template <int TColMult>
-	void initCols(const scaled_channel<TColumn, TColMult> columnBins[TColNum]) {
+	void initCols(const scaled_channel<TColumn, TColMult> (&columnBins)[TColNum]) {
-		m_columnBins = reinterpret_cast<const TColumn*>(&columnBins[0]);
+            m_columnBins = reinterpret_cast<const TColumn (*)[TColNum]>(&columnBins);
 		m_colMult = TColMult;
 	}
 
-	void initCols(const TColumn columnBins[TColNum]) {
+    void initCols(const TColumn (&columnBins)[TColNum]) {
-		m_columnBins = &columnBins[0];
+		m_columnBins = &columnBins;
 		m_colMult = 1;
 	}
 
@@ -120,10 +108,10 @@ private:
 	}
 
 	// TODO: should be const
-	/*const*/ TValue* m_values = nullptr;
+    /*const*/ TValue (*m_values)[TRowNum][TColNum] = nullptr;
 
-	const TRow *m_rowBins = nullptr;
+    const TRow (*m_rowBins)[TRowNum] = nullptr;
-	const TColumn *m_columnBins = nullptr;
+    const TColumn (*m_columnBins)[TColNum] = nullptr;
 
 	float m_rowMult = 1;
 	float m_colMult = 1;

firmware/util/math/interpolation.h

@@ -51,7 +51,7 @@ struct BinResult
  * and how far from (idx) to (idx + 1) the value is located.
  */
 template<class TBin, int TSize>
-BinResult getBinPtr(float value, const TBin* bins) {
+BinResult getBin(float value, const TBin (&bins)[TSize]) {
 	// Enforce numeric only (int, float, uintx_t, etc)
 	static_assert(std::is_arithmetic_v<TBin>, "Table bins must be an arithmetic type");
 
@@ -94,21 +94,9 @@ BinResult getBinPtr(float value, const TBin* bins) {
 	return { idx, fraction };
 }
 
-template<class TBin, int TSize, int TMult>
-BinResult getBinPtr(float value, const scaled_channel<TBin, TMult>* bins) {
-	// Strip off the scaled_channel, and perform the scaling before searching the array
-	auto binPtrRaw = reinterpret_cast<const TBin*>(bins);
-	return getBinPtr<TBin, TSize>(value * TMult, binPtrRaw);
-}
-
-template<class TBin, int TSize>
-BinResult getBin(float value, const TBin (&bins)[TSize]) {
-	return getBinPtr<TBin, TSize>(value, &bins[0]);
-}
-
 template<class TBin, int TSize, int TMult>
 BinResult getBin(float value, const scaled_channel<TBin, TMult> (&bins)[TSize]) {
-	return getBinPtr<TBin, TSize, TMult>(value, &bins[0]);
+    return getBin(value * TMult, *reinterpret_cast<const TBin (*)[TSize]>(&bins));
 }
 
 static float linterp(float low, float high, float frac)
@@ -132,6 +120,29 @@ float interpolate2d(const float value, const TBin (&bin)[TSize], const TValue (&
 	return priv::linterp(low, high, frac);
 }
 
+template<typename VType, unsigned RNum, typename RType, unsigned CNum, typename CType>
+float interpolate3d(const VType (&table)[RNum][CNum],
+                    const RType (&rowBins)[RNum], float rowValue,
+                    const CType (&colBins)[CNum], float colValue)
+{
+    auto row = priv::getBin(rowValue, rowBins);
+    auto col = priv::getBin(colValue, colBins);
+
+    // Orient the table such that (0, 0) is the bottom left corner,
+    // then the following variable names will make sense
+    float lowerLeft  = table[row.Idx    ][col.Idx    ];
+    float upperLeft  = table[row.Idx + 1][col.Idx    ];
+    float lowerRight = table[row.Idx    ][col.Idx + 1];
+    float upperRight = table[row.Idx + 1][col.Idx + 1];
+
+    // Interpolate each side by itself
+    float left  = priv::linterp(lowerLeft, upperLeft, row.Frac);
+    float right = priv::linterp(lowerRight, upperRight, row.Frac);
+
+    // Then interpolate between those
+    return priv::linterp(left, right, col.Frac);
+}
+
 /** @brief	Binary search
  * @returns	the highest index within sorted array such that array[i] is greater than or equal to the parameter
  * @note If the parameter is smaller than the first element of the array, -1 is returned.