2 files changed, 51 insertions, 1 deletions

diff --git a/lib/core/c++/math/n_linear.hpp b/lib/core/c++/math/n_linear.hpp
index ac02da8..8fb0600 100644
--- a/lib/core/c++/math/n_linear.hpp
+++ b/lib/core/c++/math/n_linear.hpp
@@ -43,6 +43,25 @@ auto floor_index_from_position(const saw::data<sch::Vector<T,D>>& pos){
 	return position_to_index_and_fraction(pos).template get<0u>();
 }
 
+template<typename T, uint64_t D>
+saw::data<sch::Tuple<sch::Vector<sch::UInt64,D>,sch::Vector<T,D>>> position_to_index_and_fraction_bounded(
+	const saw::data<sch::Vector<T,D>>& pos,
+	const saw::data<sch::Vector<sch::UInt64,D>>& bound)
+{
+	auto infr = position_to_index_and_fraction(pos);
+	auto& ind = infr.template get<0u>();
+	auto& fra = infr.template get<1u>();
+	for(uint64_t i = 0u; i < D; ++i){
+		// If index is higher than bound. Set to bound and reset fraction
+		if((ind.at({{i}}).get()+1u) >= bound.at({{i}}).get()){
+			ind.at({{i}}).set(bound.at({{i}}).get()-1u);
+			fra.at({{i}}) = {};
+		}
+	}
+	return infr;
+}
+
+
 template<typename FieldSchema, typename T, uint64_t D>
 auto n_linear_interpolate(
 		const saw::data<FieldSchema>& field, const saw::data<sch::Vector<T,D>>& pos){
diff --git a/lib/core/tests/math.cpp b/lib/core/tests/math.cpp
index 970004b..d456ce8 100644
--- a/lib/core/tests/math.cpp
+++ b/lib/core/tests/math.cpp
@@ -20,7 +20,7 @@ SAW_TEST("Math 1-Linear"){
 	pos.at({{0u}}).set(0.3);
 }
 
-SAW_TEST("Math/Floor Index from Pos"){
+SAW_TEST("Math/Floor Index and Fraction from Position"){
 	using namespace kel;
 
 	saw::data<sch::Vector<sch::Float64,2u>> pos;
@@ -45,4 +45,35 @@ SAW_TEST("Math/Floor Index from Pos"){
 	SAW_EXPECT(true, "Default true check");
 }
 
+SAW_TEST("Math/Floor Index and Fraction from Position and Bounded"){
+	using namespace kel;
+
+	saw::data<sch::Vector<sch::Float64,2u>> pos;
+
+	{
+		pos.at({{0u}}) = 43.999;
+		pos.at({{1u}}) = -50.0;
+	}
+
+	saw::data<sch::Vector<sch::UInt64,2U>> bound;
+	{
+		bound.at({{0u}}) = 32u;
+		bound.at({{1u}}) = 16u;
+	}
+
+	auto ind_frac = lbm::position_to_index_and_fraction_bounded(pos,bound);
+	auto& ind = ind_frac.template get<0u>();
+	for(uint64_t i = 0u; i < 2u; ++i){
+		std::cout<<ind.at({{i}}).get()<<" ";
+	}
+	std::cout<<std::endl;
+	auto& frac = ind_frac.template get<1u>();
+	for(uint64_t i = 0u; i < 2u; ++i){
+		std::cout<<frac.at({{i}}).get()<<" ";
+	}
+	std::cout<<std::endl;
+
+	SAW_EXPECT(true, "Default true check");
+}
+
 }