Merge branch 'dev'

11 files changed, 397 insertions, 25 deletions

diff --git a/modules/core/c++/abstract/data.hpp b/modules/core/c++/abstract/data.hpp
index f1ae5a7..327fb63 100644
--- a/modules/core/c++/abstract/data.hpp
+++ b/modules/core/c++/abstract/data.hpp
@@ -10,8 +10,97 @@ class data final {};
 
 template<typename T, uint64_t N>
 class data<sch::Primitive<T,N>,Encode> final {
+public:
+	using Schema = sch::Primitive<T,N>;
+	using Encode = Encode;
 private:
+	native_data_type<Schema>::type value_;
 public:
+	data():
+		value_{}
+	{}
+
+	data(native_data_type<Schema>::type value__):
+		value_{value__}
+	{}
+
+	constexpr auto get() const {
+		return value_;
+	}
+
+	void set(native_data_type value__){
+		value_ = value__;
+	}
+
+	constexpr bool operator==(const data<Schema,Encode>& rhs) const {
+		return value_ == rhs.value_;
+	}
+	
+	constexpr bool operator!=(const data<Schema,Encode>& rhs) const {
+		return value_ != rhs.value_;
+	}
+	
+	constexpr bool operator>(const data<Schema,Encode>& rhs) const {
+		return value_ > rhs.value_;
+	}
+	
+	constexpr bool operator<(const data<Schema,Encode>& rhs) const {
+		return value_ < rhs.value_;
+	}
+	
+	constexpr bool operator>=(const data<Schema,Encode>& rhs) const {
+		return value_ >= rhs.value_;
+	}
+	
+	constexpr bool operator<=(const data<Schema,Encode>& rhs) const {
+		return value_ >= rhs.value_;
+	}
+	
+	constexpr bool equals(const data<Schema,Encode>& rhs) const {
+		return (*this) == rhs;
+	}
+
+	data<Schema,Encode> operator+(const data<Schema,Encode>& rhs) const {
+		return {value_ + rhs.value_};
+	}
+	
+	data<Schema,Encode> operator-(const data<Schema,Encode>& rhs) const {
+		return {value_ - rhs.value_};
+	}
+	
+	data<Schema,Encode> operator*(const data<Schema,Encode>& rhs) const {
+		return {value_ * rhs.value_};
+	}
+	
+	data<Schema,Encode> operator/(const data<Schema,Encode>& rhs) const {
+		return {value_ / rhs.value_};
+	}
+	
+	data<Schema,Encode>& operator+=(const data<Schema,Encode>& rhs) {
+		value_ += rhs.value_;
+		return *this
+	}
+	
+	data<Schema,Encode>& operator-=(const data<Schema,Encode>& rhs) {
+		value_ -= rhs.value_;
+		return *this
+	}
+	
+	data<Schema,Encode>& operator*=(const data<Schema,Encode>& rhs) {
+		value_ *= rhs.value_;
+		return *this
+	}
+	
+	data<Schema,Encode>& operator/=(const data<Schema,Encode>& rhs) {
+		value_ /= rhs.value_;
+		return *this
+	}
+
+	template<typename T>
+	data<T,Encode> cast_to() const {
+		data<T,Encode> val{static_cast<typename native_data_type<T>::type>(value_)};
+		return val;
+	}
 };
 
 
diff --git a/modules/core/c++/abstract/error.hpp b/modules/core/c++/abstract/error.hpp
index 19917de..94a2c35 100644
--- a/modules/core/c++/abstract/error.hpp
+++ b/modules/core/c++/abstract/error.hpp
@@ -305,7 +305,7 @@ public:
 
 /**
  * This tries to catch cases where error starts including itself as a type which can happen in more complicated cases.
- * So this acts as a type safe guard.
+ * So this acts as a type safe guard in case internal handling fails to treat conversion correctly.
  */
 template <typename T> class error_or<error_or<T>> {
 private:
diff --git a/modules/core/c++/fplbm.hpp b/modules/core/c++/fplbm.hpp
index cfb0062..1f38f51 100644
--- a/modules/core/c++/fplbm.hpp
+++ b/modules/core/c++/fplbm.hpp
@@ -10,6 +10,7 @@ namespace cmpt {
 struct FpLbmReset{};
 struct FpLbm {};
 struct FpLbmOneParticle {};
+struct FpLbmOneParticleImplicit {};
 struct FpLbmOneParticleNoVelocity{};
 }
 namespace method {
@@ -36,18 +37,20 @@ public:
 		for(uint64_t i{0u}; i < Descriptor::D; ++i){
 			force.at({{i}}) = 0.0;
 		}
+		auto info = field.template get<"info">().at(index).get();
+		if(info == 2u){
+			bool is_even = ((time_step.get() % 2) == 0);
 
-		bool is_even = ((time_step.get() % 2) == 0);
-
-		auto& dfs_old_f = (is_even) ? field.template get<"dfs_old">() : field.template get<"dfs">();
-		auto& dfs = dfs_old_f.at(index);
+			auto& dfs_old_f = (is_even) ? field.template get<"dfs_old">() : field.template get<"dfs">();
+			auto& dfs = dfs_old_f.at(index);
 
-		auto& rho_f = macros.template get<"density">();
-		auto& rho = rho_f.at(index);
-		auto& vel_f = macros.template get<"velocity">();
-		auto& vel = vel_f.at(index);
+			auto& rho_f = macros.template get<"density">();
+			auto& rho = rho_f.at(index);
+			auto& vel_f = macros.template get<"velocity">();
+			auto& vel = vel_f.at(index);
 
-		compute_rho_u<T,Descriptor>(dfs,rho,vel);
+			compute_rho_u<T,Descriptor>(dfs,rho,vel);
+		}
 	}
 };
 
@@ -83,7 +86,7 @@ public:
 		auto& vel_f = macros.template get<"velocity">();
 		saw::data<sch::Vector<T,Descriptor::D>>& vel = vel_f.at(index);
 
-		compute_rho_u<T,Descriptor>(dfs,rho,vel);
+		// compute_rho_u<T,Descriptor>(dfs,rho,vel);
 		auto eq = equilibrium<T,Descriptor>(rho,vel);
 
 		using dfi = df_info<T,Descriptor>;
@@ -158,10 +161,94 @@ public:
 		auto& pirb_pos = pirb.template get<"position">();
 		auto& pirb_pos_old = pirb.template get<"position_old">();
 		
+
+		auto& p_coll = pg.template get<"collision">().at({});
+		auto& p_rad = p_coll.template get<"radius">();
+
+		auto eo_aabb = particle_aabb<typename ParticleSchema::ValueType>::calculate(pg,{{0u}},vel_f.meta());
+		if(eo_aabb.is_error()){
+			return;
+		}
+		auto& aabb = eo_aabb.get_value();
+
+		saw::data<sch::Scalar<T>> two;
+		two.at({}).set(2.0f);
+
+		saw::data<sch::Scalar<T>> one;
+		one.at({}) = 1.0;
+		
+		saw::data<sch::Scalar<T>> eps;
+		eps.at({}) = 1.5f;
+		
 		saw::data<sch::Scalar<T>> sss;
-		sss.at({}) = sub_steps.template cast_to<T>();
+		sss.at({}) = one.at({}) / sub_steps.template cast_to<T>();
 		auto vel_s = (pirb_pos-pirb_pos_old) / sss;
 
+		saw::data<sch::Vector<T,Descriptor::D>> force_p{};
+
+		iterator<Descriptor::D>::apply([&](const auto& index_f) -> void {
+			auto& force = force_f.at(index_f);
+			auto& vel = vel_f.at(index_f);
+			auto& por = por_f.at(index_f);
+			auto& rho = rho_f.at(index_f);
+
+			saw::data<sch::Vector<T,Descriptor::D>> rel_dist = saw::math::vectorize_data(index_f).template cast_to<T>() - pirb_pos;
+
+			por = particle_porosity<T,Descriptor::D,1u,por::ParticleSpheroid<T>>::calculate(rel_dist,p_rad,eps);
+			auto flip_por = one - por;
+
+			// vel_s is technically time the density of the particle?
+
+			force = ( vel_s * rho - vel * rho ) * two * flip_por;
+
+			force_p = force_p - force;
+		}, aabb.template get<"a">(), aabb.template get<"b">());
+
+		auto& pirb_acc = pirb.template get<"acceleration">();
+		pirb_acc = force_p / pg.template get<"total_mass">().at({});
+
+		for(saw::data<sch::UInt64> i{0u}; i < sub_steps; ++i){
+			verlet_step_lambda<T,Descriptor::D>(pi,sss);
+		}
+	}
+};
+
+template<typename T, typename Descriptor, typename Encode>
+class component<T, Descriptor, cmpt::FpLbmOneParticleImplicit, Encode> final {
+public:
+	using Component = cmpt::FpLbmOneParticle;
+private:
+public:
+	component() = default;
+
+	template<typename CellFieldSchema, typename MacroFieldSchema, typename ParticleSchema>
+	void apply(const saw::data<CellFieldSchema, Encode>& field, const saw::data<MacroFieldSchema,Encode>& macros, const saw::data<ParticleSchema,Encode>& pg, saw::data<sch::FixedArray<sch::UInt64,1u>> index, saw::data<sch::UInt64> time_step, saw::data<sch::UInt64> sub_steps) const {
+	// void apply(saw::data<CellFieldSchema, Encode>& field, saw::data<sch::FixedArray<sch::UInt64, Descriptor::D>> index, saw::data<sch::UInt64> time_step){
+		bool is_even = ((time_step.get() % 2) == 0);
+
+		//auto& dfs_old_f = (is_even) ? field.template get<"dfs_old">() : field.template get<"dfs">();
+		// auto& dfs = dfs_old_f.at(index);
+
+		auto& rho_f = macros.template get<"density">();
+		auto& vel_f = macros.template get<"velocity">();
+		auto& por_f = macros.template get<"porosity">();
+		auto& force_f = macros.template get<"force">();
+
+		/**
+		 * The other methods all work with a flipped porosity.
+		 * For compat reasons this is also flipped
+		 */
+		// TODO - Change to tuple later
+		auto parts = pg.template get<"particles">();
+
+		auto parts_size = parts.meta().at({0u});
+		auto& pi = parts.at(index);		
+
+		auto& pirb = pi.template get<"rigid_body">();
+		auto& pirb_pos = pirb.template get<"position">();
+		auto& pirb_pos_old = pirb.template get<"position_old">();
+		
+
 		auto& p_coll = pg.template get<"collision">().at({});
 		auto& p_rad = p_coll.template get<"radius">();
 
@@ -172,15 +259,19 @@ public:
 		auto& aabb = eo_aabb.get_value();
 
 		saw::data<sch::Scalar<T>> two;
-		two.at({}).set(2);
+		two.at({}).set(2.0f);
 
 		saw::data<sch::Scalar<T>> one;
 		one.at({}) = 1.0;
 		
 		saw::data<sch::Scalar<T>> eps;
 		eps.at({}) = 1.5f;
+		
+		saw::data<sch::Scalar<T>> sss;
+		sss.at({}) = one.at({}) / sub_steps.template cast_to<T>();
+		auto vel_s = (pirb_pos-pirb_pos_old) / sss;
 
-		saw::data<sch::Vector<T,Descriptor::D>> force_p;
+		saw::data<sch::Vector<T,Descriptor::D>> force_p{};
 
 		iterator<Descriptor::D>::apply([&](const auto& index_f) -> void {
 			auto& force = force_f.at(index_f);
@@ -193,13 +284,15 @@ public:
 			por = particle_porosity<T,Descriptor::D,1u,por::ParticleSpheroid<T>>::calculate(rel_dist,p_rad,eps);
 			auto flip_por = one - por;
 
-			force = ( vel_s * rho -  vel * rho ) * two * flip_por;
+			// vel_s is technically time the density of the particle?
+
+			force = ( vel_s * rho - vel * rho ) * two * flip_por / (one + flip_por);
 
-			force_p = force_p + force;
+			force_p = force_p - force;
 		}, aabb.template get<"a">(), aabb.template get<"b">());
 
 		auto& pirb_acc = pirb.template get<"acceleration">();
-		pirb_acc = - force_p / pg.template get<"total_mass">().at({});
+		pirb_acc = force_p / pg.template get<"total_mass">().at({});
 
 		for(saw::data<sch::UInt64> i{0u}; i < sub_steps; ++i){
 			verlet_step_lambda<T,Descriptor::D>(pi,sss);
diff --git a/modules/core/c++/particle/aabb.hpp b/modules/core/c++/particle/aabb.hpp
index da39ec2..e4c930b 100644
--- a/modules/core/c++/particle/aabb.hpp
+++ b/modules/core/c++/particle/aabb.hpp
@@ -26,7 +26,6 @@ public:
 	static constexpr saw::error_or<saw::data<AABB>> calculate(const saw::data<Sch,Encode>& p_grp, const saw::data<sch::FixedArray<sch::UInt64,1u>>& index, const saw::data<sch::FixedArray<sch::UInt64,D>>& meta){
 		static_assert(PC > 0u, "Can't calculate from no particles");
 		if(not (index.at({{0u}}).get() < PC) ){
-			std::cerr.flush();
 			return saw::make_error<saw::err::critical>("Too large i in particle_aabb");
 		}
 
diff --git a/modules/core/c++/particle/cuboid/aabb.hpp b/modules/core/c++/particle/cuboid/aabb.hpp
new file mode 100644
index 0000000..502132a
--- /dev/null
+++ b/modules/core/c++/particle/cuboid/aabb.hpp
@@ -0,0 +1,40 @@
+#pragma once
+
+#include "common.hpp"
+#include "../aabb.hpp"
+
+namespace kel {
+namespace lbm {
+
+template<typename T, uint64_t D, uint64_t PC>
+class particle_aabb<
+	sch::ParticleGroup<T,D,PC,coll::Cuboid<T>>
+> final {
+public:
+	using Schema = sch::ParticleGroup<T,D,PC,coll::Cuboid<T>>;
+
+	using AABB = sch::Struct<
+		sch::Member<sch::FixedArray<sch::UInt64,D>, "a">,
+		sch::Member<sch::FixedArray<sch::UInt64,D>, "b">
+	>;
+
+public:
+	template<typename Sch, typename Encode>
+	static constexpr saw::error_or<saw::data<AABB>> calculate(const saw::data<Sch,Encode>& pg, const saw::data<sch::FixedArray<sch::UInt64,1u>>& index, const saw::data<sch::FixedArray<sch::UInt64,D>>& meta){
+		static_assert(PC > 0u, "Can't calculate from no particle");
+		if(not (index.at({0u}).get() < PC)){
+			return saw::make_error<saw::err::critical>("Too large i in particle_aabb coll::Cuboid<T>");
+		}
+
+		saw::data<AABB> aabb;
+		auto& parts = pg.template get<"particles">();
+		auto& pi = parts.at(index);
+		auto& pirb = pi.template get<"rigid_body">();
+
+		/// TODO
+
+		return aabb;
+	}
+};
+}
+}
diff --git a/modules/core/c++/particle/cuboid/common.hpp b/modules/core/c++/particle/cuboid/common.hpp
new file mode 100644
index 0000000..a859cbe
--- /dev/null
+++ b/modules/core/c++/particle/cuboid/common.hpp
@@ -0,0 +1,10 @@
+#pragma once
+
+namespace kel {
+namespace lbm {
+namespace coll {
+template<typename T>
+struct Cuboid {};
+}
+}
+}
diff --git a/modules/core/c++/particle/cuboid/particle.hpp b/modules/core/c++/particle/cuboid/particle.hpp
new file mode 100644
index 0000000..e19d543
--- /dev/null
+++ b/modules/core/c++/particle/cuboid/particle.hpp
@@ -0,0 +1,9 @@
+#pragma once
+
+#include "common.hpp"
+#include "porosity.hpp"
+
+namespace kel {
+namespace lbm {
+}
+}
diff --git a/modules/core/c++/particle/cuboid/porosity.hpp b/modules/core/c++/particle/cuboid/porosity.hpp
new file mode 100644
index 0000000..bccfebf
--- /dev/null
+++ b/modules/core/c++/particle/cuboid/porosity.hpp
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "common.hpp"
+#include "../porosity.hpp"
+
+namespace kel {
+namespace lbm {
+
+}
+}
diff --git a/modules/core/c++/particle/particle.hpp b/modules/core/c++/particle/particle.hpp
index a3669b4..21b4b34 100644
--- a/modules/core/c++/particle/particle.hpp
+++ b/modules/core/c++/particle/particle.hpp
@@ -82,8 +82,8 @@ saw::data<sch::ParticleGroup<T, D, PartAmount, coll::Spheroid<T>>> create_sphero
 		total_mass.at({}) = rad_d * rad_d * density.at({}) * 3.141592;
 	}else if constexpr ( D == 3u ){
 		
-	}else if constexpr ( D== 1u ){
-
+	}else if constexpr ( D == 1u ){
+		total_mass.at({}) = rad_d * 2.0;
 	}
 	std::cout<<"Total Mass: "<<total_mass.at({}).get()<<std::endl;
 
diff --git a/modules/core/c++/particle/spheroid/particle.hpp b/modules/core/c++/particle/spheroid/particle.hpp
new file mode 100644
index 0000000..9c0912e
--- /dev/null
+++ b/modules/core/c++/particle/spheroid/particle.hpp
@@ -0,0 +1,9 @@
+#pragma once
+
+namespace kel {
+namespace lbm {
+namespace coll {
+
+}
+}
+}
diff --git a/modules/core/c++/psm.hpp b/modules/core/c++/psm.hpp
index 02db1e1..15ad0f8 100644
--- a/modules/core/c++/psm.hpp
+++ b/modules/core/c++/psm.hpp
@@ -7,15 +7,30 @@
 namespace kel {
 namespace lbm {
 namespace cmpt {
-struct PSM {};
+struct PsmReset {};
 struct PsmOneParticle {};
+struct Psm {};
 }
 
+template<typename T, typename Descriptor, typename Encode>
+class component<T, Descriptor, cmpt::PsmReset, Encode> final {
+public:
+	component() = default;
+
+	template<typename CellFieldSchema, typename MacroFieldSchema>
+	void apply(const saw::data<CellFieldSchema, Encode>& field, const saw::data<MacroFieldSchema,Encode>& macros, saw::data<sch::FixedArray<sch::UInt64,Descriptor::D>> index, saw::data<sch::UInt64> time_step) const {
+		auto& porosity_f = macros.template get<"porosity">();
+
+		auto& por = porosity_f.at(index);
+		por.at({}) = 1.0;
+	}
+};
+
 /**
  * PSM collision operator for LBM
  */
 template<typename T, typename Descriptor, typename Encode>
-class component<T, Descriptor, cmpt::PSM, Encode> {
+class component<T, Descriptor, cmpt::Psm, Encode> {
 private:
 	saw::data<T> relaxation_;
 	saw::data<T> frequency_;
@@ -90,17 +105,115 @@ public:
 	component() = default;
 
 	template<typename CellFieldSchema, typename MacroFieldSchema, typename ParticleSchema>
-	void apply(const saw::data<CellFieldSchema, Encode>& field, const saw::data<MacroFieldSchema,Encode>& macros, const saw::data<ParticleSchema,Encode>& particles, saw::data<sch::FixedArray<sch::UInt64,1u>> index, saw::data<sch::UInt64> time_step) const {
+	void apply(const saw::data<CellFieldSchema, Encode>& field, const saw::data<MacroFieldSchema,Encode>& macros, const saw::data<ParticleSchema,Encode>& pg, saw::data<sch::FixedArray<sch::UInt64,1u>> index, saw::data<sch::UInt64> time_step, saw::data<sch::UInt64> sub_steps) const {
 	
 		using dfi = df_info<T,Descriptor>;
 		bool is_even = ((time_step.get() % 2) == 0);
+		
+		saw::data<sch::Scalar<T>> one;
+		one.at({}) = 1.0;
 
 		auto& dfs_old_f = (is_even) ? field.template get<"dfs_old">() : field.template get<"dfs">();
-		auto& porous_f = macros.template get<"porosity">();
 
+		auto& por_f = macros.template get<"porosity">();
 		auto& rho_f = macros.template get<"density">();
 		auto& vel_f = macros.template get<"velocity">();
-		
+		auto& force_f = macros.template get<"force">();
+	
+		{
+			auto parts = pg.template get<"particles">();
+			auto parts_size = parts.meta().at({0u});
+
+			auto& p_coll = pg.template get<"collision">().at({});
+			auto& p_rad = p_coll.template get<"radius">();
+
+			auto& pi = parts.at(index);
+			auto& pirb = pi.template get<"rigid_body">();
+			saw::data<sch::Vector<T,Descriptor::D>>& pirb_pos = pirb.template get<"position">();
+			auto& pirb_pos_old = pirb.template get<"position_old">();
+				
+			saw::data<sch::Scalar<T>> ts;
+			ts.at({}) = one.at({}) / sub_steps.template cast_to<T>();
+
+			saw::data<sch::FixedArray<sch::UInt64,Descriptor::D>> start;
+			saw::data<sch::FixedArray<sch::UInt64,Descriptor::D>> stop;
+
+			auto eo_aabb = particle_aabb<typename ParticleSchema::ValueType>::calculate(pg,{{0u}},vel_f.meta());
+			if(eo_aabb.is_error()){
+				return;
+			}
+			auto& aabb = eo_aabb.get_value();
+
+			/// Ok, I iterate over the space which covers our particle? So lower bounds to upper bounds
+			start = aabb.template get<"a">();
+			stop = aabb.template get<"b">();
+
+			saw::data<sch::Vector<T,Descriptor::D>> force_p;
+			for(uint64_t i{0u}; i < Descriptor::D; ++i){
+				force_p.at({{i}}) = 0.0;
+			}
+			auto vel_p_old = (pirb_pos-pirb_pos_old) / ts;
+
+			iterator<Descriptor::D>::apply([&](const auto& index_f) -> void{
+				// ask for the d_k value here.
+				// For every value im iterating over I need sth
+				// std::cout<<"Pos: "<<index.at({0u}).get()<<" "<<index.at({1u}).get()<<std::endl;
+			
+				auto& dfs = dfs_old_f.at(index_f);
+				saw::data<sch::Vector<T,Descriptor::D>> rel_dist = saw::math::vectorize_data(index_f).template cast_to<T>() - pirb_pos;
+				saw::data<sch::Scalar<T>> eps;
+				eps.at({}) = 1.5f;
+				
+				auto& por = por_f.at(index_f);
+				por = particle_porosity<T,Descriptor::D,1u,por::ParticleSpheroid<T>>::calculate(rel_dist,p_rad,eps);
+
+				if(por.at({}).get() >= 1.0f){
+					return;
+				}
+
+				saw::data<sch::Vector<T,Descriptor::D>> momentum;
+				for(uint64_t i{0u}; i < Descriptor::D; ++i){
+					momentum.at({{i}}) = 0.0;
+				}
+
+				for(uint64_t i{0u}; i < Descriptor::Q; ++i){
+					saw::data<sch::Vector<T,Descriptor::D>> e_i;
+					saw::data<sch::FixedArray<sch::UInt64,Descriptor::D>> n_ind_i;
+					for(uint64_t k{0u}; k < Descriptor::D; ++k){
+						e_i.at({{k}}) = (dfi::directions[i])[k];
+						n_ind_i.at({k}) = index_f.at({k}) + (dfi::directions[i])[k];
+					}
+
+					uint64_t i_opp = dfi::opposite_index[i];
+
+					auto u_p_e = saw::math::dot(e_i,vel_p_old);
+
+					saw::data<T> dfs_added = dfs.at({i})*(saw::data<T>{1}-u_p_e.at({})) + dfs_old_f.at(n_ind_i).at({i_opp})*(saw::data<T>{1}+u_p_e.at({}));
+					saw::data<sch::Scalar<T>> dfs_added_v;
+					dfs_added_v.at({}) = dfs_added;
+					auto ei_dfs = e_i * dfs_added_v;
+
+					momentum = momentum + ei_dfs;
+				}
+				// technically needs to adjust for rotation as well
+				
+				auto& force = force_f.at(index_f);
+				auto& rho = rho_f.at(index_f);
+				// To Fluid
+				
+				auto flip_por = one - por;
+				force = momentum * flip_por;
+				// To Particle
+				force_p = force_p - force;
+			},start,stop);
+			
+			auto& pirb_acc = pirb.template get<"acceleration">();
+			pirb_acc = force_p / pg.template get<"total_mass">().at({});
+
+			for(saw::data<sch::UInt64> i{0u}; i < sub_steps; ++i){
+				verlet_step_lambda<T,Descriptor::D>(pi,ts);
+			}
+		}
 	}
 };