Trying out n linear interpolation

1 files changed, 31 insertions, 13 deletions

diff --git a/lib/core/c++/math/n_linear.hpp b/lib/core/c++/math/n_linear.hpp
index 62906cd..4f47965 100644
--- a/lib/core/c++/math/n_linear.hpp
+++ b/lib/core/c++/math/n_linear.hpp
@@ -6,24 +6,42 @@ namespace kel {
 namespace lbm {
 
 template<typename FieldSchema, typename T, uint64_t D>
-void n_linear_interpolate(const saw::data<FieldSchema>& field, const saw::data<sch::FixedArray<T,D>>& pos){
+auto n_linear_interpolate(const saw::data<FieldSchema>& field, const saw::data<sch::Vector<T,D>>& pos){
 
+	// Pos
 	auto pos_bound = pos;
+	
+	// Dimensions
 	auto meta = field.dims();
-	saw::data<sch::UInt64> ind;
+
+	// Lower Index
+	saw::data<sch::FixedArray<sch::UInt64,D>> ind;
+
 	for(saw::data<sch::UInt64> i{0u}; i < saw::data<sch::UInt64>{D}; ++i){
-		pos_bound.at(i).set(
-			std::max(
-				std::min(
-					meta.at(i).template cast_to<T>().get(),
-					pos.at(i).get() + 1.5
-				),
-				1,
-			)
-			-1
-		);
-		ind.at(i) = pos_bound.at(i).template cast_to<sch::UInt64>();
+		// Native Positive i
+		auto npos_i = pos.at({i}).get();
+
+		{
+			// Ok I want to stay in bounds
+			npos_i = std::min(npos_i,meta.at(i).get()-1.0);
+			npos_i = std::max(npos_i,1.0);
+		}
+
+		// Native Index i
+		auto nind_i = static_cast<uint64_t>(std::floor(npos_i))-1ul;
+
+		// Set index to i
+		ind.at(i).set(nind_i);
 	}
+	saw::data<sch::Vector<T,D>> pos_frac;
+	for(saw::data<sch::UInt64> i{0u}; i < saw::data<sch::UInt64>{D}; ++i){
+		pos_frac.at({i}) = pos_bound.at({i}) - ind.at(i).template cast_to<T>();
+	}
+
+	// Base value
+	auto res = field.at(ind);
+
+	constexpr uint64_t d_corners = 1ull << D;
 }
 }
 }