Merge branch 'dev'

5 files changed, 252 insertions, 54 deletions

diff --git a/lib/core/c++/boundary.hpp b/lib/core/c++/boundary.hpp
index adb473d..4dbbdf8 100644
--- a/lib/core/c++/boundary.hpp
+++ b/lib/core/c++/boundary.hpp
@@ -174,14 +174,13 @@ class component<FP, Descriptor, cmpt::ZouHeHorizontal<Dir>, Encode> final {
 private:
 	saw::data<FP> rho_setting_;
 public:
-	component(const saw::data<FP>& density_setting__):
-		rho_setting_{density_setting__}
+	component(const saw::data<FP>& rho_setting__):
+		rho_setting_{rho_setting__}
 	{}
 
 	template<typename CellFieldSchema>
 	void apply(const saw::data<CellFieldSchema, Encode>& field, saw::data<sch::FixedArray<sch::UInt64,Descriptor::D>> index, saw::data<sch::UInt64> time_step) const {
 		using dfi = df_info<FP,Descriptor>;
-		constexpr int known_dir = Dir ? 1 : -1;
 
 		bool is_even = ((time_step.get() % 2) == 0);
 
@@ -191,42 +190,33 @@ public:
 		auto& dfs_old_f = (is_even) ? field.template get<"dfs_old">() : field.template get<"dfs">();
 
 		auto& dfs_old = dfs_old_f.at(index);
-		/**
-		 * Sum all known DFs
-		 */
-		saw::data<FP> sum_df{0};
-		for(saw::data<sch::UInt64> k{0u}; k < saw::data<sch::UInt64>{Descriptor::Q}; ++k){
-			auto c_k = dfi::directions[k.get()];
-
-			if(c_k[0u]*known_dir <= 0){
-				sum_df += dfs_old.at({k});
-			}
-		}
-
-		/**
-		 * Get the sum of the unknown dfs and precalculate the direction
-		 */
-		auto sum_unknown_dfs = (rho_setting_ - sum_df) * saw::data<FP>{known_dir};
 
-		for(saw::data<sch::UInt64> k{0u}; k < saw::data<sch::UInt64>{Descriptor::Q}; ++k){
-			auto c_k = dfi::directions[k.get()];
-			if(c_k[0u]*known_dir < 0){
-				sum_unknown_dfs += dfs_old.at({k}) * c_k[0u];
+		auto rho_vel_x = [&]() -> saw::data<FP> {
+			if constexpr (Dir){
+				auto S = dfs_old.at({0u})
+					+ dfs_old.at({3u}) + dfs_old.at({4u})
+					+ (dfs_old.at({1u}) + dfs_old.at({5u}) + dfs_old.at({7u})) * 2u;
+				return rho_setting_ - S;
 			}
-		}
-
-		auto vel_x = sum_unknown_dfs / rho_setting_;
+			else if constexpr (not Dir) {
+				auto S = dfs_old.at({0u})
+					+ dfs_old.at({3u}) + dfs_old.at({4u})
+					+ (dfs_old.at({2u}) + dfs_old.at({6u}) + dfs_old.at({8u})) * 2u;
+				return S - rho_setting_;
+			}
+			return {};
+		}();
 
 		static_assert(Descriptor::D == 2u and Descriptor::Q == 9u, "Some parts are hard coded sadly");
 
 		if constexpr (Dir) {
-			dfs_old.at({2u}) = dfs_old.at({1u}) + saw::data<FP>{2.0 / 3.0} * rho_setting_ * vel_x;
-			dfs_old.at({6u}) = dfs_old.at({5u}) + saw::data<FP>{1.0 / 6.0} * rho_setting_ * vel_x + saw::data<FP>{0.5} * (dfs_old.at({4u}) - dfs_old.at({3u}));
-			dfs_old.at({8u}) = dfs_old.at({7u}) + saw::data<FP>{1.0 / 6.0} * rho_setting_ * vel_x + saw::data<FP>{0.5} * (dfs_old.at({3u}) - dfs_old.at({4u}));
+			dfs_old.at({2u}) = dfs_old.at({1u}) + saw::data<FP>{2.0 / 3.0} * rho_vel_x;
+			dfs_old.at({6u}) = dfs_old.at({5u}) + saw::data<FP>{1.0 / 6.0} * rho_vel_x + saw::data<FP>{0.5} * (dfs_old.at({4u}) - dfs_old.at({3u}));
+			dfs_old.at({8u}) = dfs_old.at({7u}) + saw::data<FP>{1.0 / 6.0} * rho_vel_x + saw::data<FP>{0.5} * (dfs_old.at({3u}) - dfs_old.at({4u}));
 		}else if constexpr (not Dir){
-			dfs_old.at({1u}) = dfs_old.at({2u}) - saw::data<FP>{2.0 / 3.0} * rho_setting_ * vel_x;
-			dfs_old.at({5u}) = dfs_old.at({6u}) - saw::data<FP>{1.0 / 6.0} * rho_setting_ * vel_x + saw::data<FP>{0.5} * (dfs_old.at({3u}) - dfs_old.at({4u}));
-			dfs_old.at({7u}) = dfs_old.at({8u}) - saw::data<FP>{1.0 / 6.0} * rho_setting_ * vel_x + saw::data<FP>{0.5} * (dfs_old.at({4u}) - dfs_old.at({3u}));
+			dfs_old.at({1u}) = dfs_old.at({2u}) - saw::data<FP>{2.0 / 3.0} * rho_vel_x;
+			dfs_old.at({5u}) = dfs_old.at({6u}) - saw::data<FP>{1.0 / 6.0} * rho_vel_x + saw::data<FP>{0.5} * (dfs_old.at({3u}) - dfs_old.at({4u}));
+			dfs_old.at({7u}) = dfs_old.at({8u}) - saw::data<FP>{1.0 / 6.0} * rho_vel_x + saw::data<FP>{0.5} * (dfs_old.at({4u}) - dfs_old.at({3u}));
 		}
 	}
 };
diff --git a/lib/core/c++/descriptor.hpp b/lib/core/c++/descriptor.hpp
index 73f0cce..9f7399a 100644
--- a/lib/core/c++/descriptor.hpp
+++ b/lib/core/c++/descriptor.hpp
@@ -146,15 +146,15 @@ public:
 	static constexpr uint64_t Q = 9u;
 
 	static constexpr std::array<std::array<int32_t, D>, Q> directions = {{
-		{ 0, 0},
-		{-1, 0},
-		{ 1, 0},
-		{ 0,-1},
-		{ 0, 1},
-		{-1,-1},
-		{ 1, 1},
-		{-1, 1},
-		{ 1,-1}
+		{ 0, 0}, // 0
+		{-1, 0}, // 1
+		{ 1, 0}, // 2
+		{ 0,-1}, // 3
+		{ 0, 1}, // 4
+		{-1,-1}, // 5
+		{ 1, 1}, // 6
+		{-1, 1}, // 7
+		{ 1,-1}  // 8
 	}};
 
 	static constexpr std::array<typename saw::native_data_type<T>::type,Q> weights = {
diff --git a/lib/core/c++/math/n_linear.hpp b/lib/core/c++/math/n_linear.hpp
index 62906cd..8fb0600 100644
--- a/lib/core/c++/math/n_linear.hpp
+++ b/lib/core/c++/math/n_linear.hpp
@@ -1,29 +1,136 @@
 #pragma once
 
 #include "../common.hpp"
+#include "../iterator.hpp"
 
 namespace kel {
 namespace lbm {
+namespace impl {
+template<typename FieldSchema, typename T, uint64_t D>
+struct n_linear_interpolate_helper final {
+	template<uint64_t i = 0u>
+	auto apply(const saw::data<FieldSchema>& field, const saw::data<sch::Vector<T,D>>& pos){
+		return pos;
+	}
+};
+}
+
+template<typename T, uint64_t D>
+saw::data<sch::Tuple<sch::Vector<sch::UInt64,D>,sch::Vector<T,D>>> position_to_index_and_fraction(const saw::data<sch::Vector<T,D>>& pos){
+	saw::data<sch::Tuple<sch::Vector<sch::UInt64,D>,sch::Vector<T,D>>> sep;
+
+	auto& ind = sep.template get<0u>();
+	auto& frac = sep.template get<1u>();
+
+	auto pos_cpy = pos;
+	// Guarantee that the pos is at least 0
+	for(uint64_t i = 0u; i < D; ++i){
+		pos_cpy.at({{i}}).set(std::max(pos.at({{i}}).get(), static_cast<typename saw::native_data_type<T>::type>(0)));
+	}
+
+	// Now we can cast to uint64_t
+	for(uint64_t i = 0u; i < D; ++i){
+		ind.at({{i}}) = pos_cpy.at({{i}}).template cast_to<sch::UInt64>();
+	}
+
+	frac = pos_cpy - ind.template cast_to<T>();
+
+	return sep;
+}
+
+template<typename T, uint64_t D>
+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>
-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
+	saw::data<typename FieldSchema::ValueType> res;
+
+	// constexpr uint64_t d_corners = 1ul << D;
+
+	saw::data<sch::FixedArray<sch::UInt64,D>> ones_ind;
+	for(saw::data<sch::UInt64> i{0u}; i < saw::data<sch::UInt64>{D}; ++i){
+		ones_ind.at({i}).set(1u);
+	}
+
+	iterator<D>::apply([&](auto ind){
+		// Iterates over (0,0,0) to (1,1,1)
+		saw::data<T> weight{1.0};
+
+		for(saw::data<sch::UInt64> d{0u}; d < saw::data<sch::UInt64>{D}; ++d){
+
+			saw::data<T> t = pos_frac.at({d});
+
+			if(ind.at(d).get() == 0u){
+				weight = weight * (saw::data<T>{1} - t);
+			}else{
+				weight = weight * t;
+			}
+		}
+
+	}, {}, ones_ind);
+}
+
+template<typename FieldSchema, typename T>
+saw::data<sch::Vector<T,2u>> bilinear_interpolate(const saw::data<FieldSchema>& field, const saw::data<sch::Vector<T,2u>>& pos){
+	saw::data<sch::Vector<T,2u>> res;
+
+	{
 	}
+	return {};
 }
 }
 }
diff --git a/lib/core/c++/particle/geometry/cube.hpp b/lib/core/c++/particle/geometry/cube.hpp
new file mode 100644
index 0000000..6392de8
--- /dev/null
+++ b/lib/core/c++/particle/geometry/cube.hpp
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "../particle.hpp"
+
+namespace kel {
+namespace lbm {
+template<typename T, uint64_t D>
+class particle_cubic_geometry {
+private:
+public:
+	template<typename MT = T>
+	saw::data<sch::ParticleMask<MT,D>> generate_mask(uint64_t resolution, uint64_t bound_nodes = 0u){
+
+		saw::data<sch::ParticleMask<MT,D>> mask;
+
+		auto& grid = mask.template get<"grid">();
+		auto& com = mask.template get<"center_of_mass">();
+
+
+	}
+}
+}
diff --git a/lib/core/tests/math.cpp b/lib/core/tests/math.cpp
new file mode 100644
index 0000000..d456ce8
--- /dev/null
+++ b/lib/core/tests/math.cpp
@@ -0,0 +1,79 @@
+#include <forstio/test/suite.hpp>
+
+#include <iostream>
+#include "../c++/math/n_linear.hpp"
+
+namespace {
+namespace sch {
+using namespace saw::schema;
+}
+
+SAW_TEST("Math 1-Linear"){
+	using namespace kel;
+	
+	saw::data<sch::FixedArray<sch::Vector<sch::Float64,1u>,2u>> field;
+	{
+		field.at({{0u}}).at({{0u}}).set(-1.0);
+		field.at({{1u}}).at({{0u}}).set(2.0);
+	}
+	saw::data<sch::Vector<sch::Float64,1u>> pos;
+	pos.at({{0u}}).set(0.3);
+}
+
+SAW_TEST("Math/Floor Index and Fraction from Position"){
+	using namespace kel;
+
+	saw::data<sch::Vector<sch::Float64,2u>> pos;
+
+	{
+		pos.at({{0u}}) = 43.999;
+		pos.at({{1u}}) = -50.0;
+	}
+
+	auto ind_frac = lbm::position_to_index_and_fraction(pos);
+	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");
+}
+
+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");
+}
+
+}