Asking myself how I had a stable implementation before

2 files changed, 170 insertions, 17 deletions

diff --git a/modules/core/c++/fplbm.hpp b/modules/core/c++/fplbm.hpp
index cfb0062..4acdd36 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,16 @@ 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 ) * flip_por / (one + flip_por / two);
+			// force = ( vel_s * rho - vel * rho ) * two * 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++/psm.hpp b/modules/core/c++/psm.hpp
index 02db1e1..c53dbf9 100644
--- a/modules/core/c++/psm.hpp
+++ b/modules/core/c++/psm.hpp
@@ -86,8 +86,18 @@ public:
 template<typename T, typename Descriptor, typename Encode>
 class component<T, Descriptor, cmpt::PsmOneParticle, Encode> final {
 private:
+	saw::data<T> relaxation_;
+	saw::data<T> frequency_;
 public:
-	component() = default;
+	component(
+		typename saw::native_data_type<T>::type relaxation__
+	):
+		relaxation_{relaxation__}
+	{
+		saw::data<T> one;
+		one = 1.0;
+		frequency_ = one / relaxation_;
+	}
 
 	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 {
@@ -101,6 +111,55 @@ public:
 		auto& rho_f = macros.template get<"density">();
 		auto& vel_f = macros.template get<"velocity">();
 		
+		saw::data<sch::Scalar<T>>& rho = rho_f.at(index);
+		saw::data<sch::Vector<T,Descriptor::D>>& vel = vel_f.at(index);
+
+		compute_rho_u<T,Descriptor>(dfs_old_f.at(index),rho,vel);
+		
+		auto eq = equilibrium<T,Descriptor>(rho,vel);
+
+		saw::data<T> one{1.0};
+		auto& porous = porous_f.at(index);
+		auto flip_porous = one - porous.at({});
+
+		auto& dfs = dfs_old_f.at(index);
+
+		auto dfs_cpy = dfs;
+
+		for(uint64_t i = 0u; i < Descriptor::Q; ++i){
+			uint64_t i_opp = dfi::opposite_index[i];
+			dfs.at({i}) = dfs_cpy.at({i}) + frequency_ * (eq.at(i) - dfs_cpy.at({i})) * porous.at({}) + (dfs_cpy.at({i_opp}) - dfs_cpy.at({i}) ) * flip_porous;
+		}
+
+		auto& force_f = macros.template get<"force">();
+		auto& force = force_f.at(index);
+		for(uint64_t k{0u}; k < Descriptor::D; ++k){
+			force.at({{k}}).set(0);
+		}
+
+		/////////
+		saw::data<sch::Vector<T,Descriptor::D>> momentum;
+		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}) = (dfi::directions[i])[k];
+			}
+
+			uint64_t i_opp = dfi::opposite_index[i];
+
+			saw::data<T> dfs_added = dfs.at({i}) - dfs_old_f.at(n_ind_i).at({i_opp});
+			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;
+		}
+		
+		// Set Force
+		force = momentum * porous_f.at(index);
 	}
 };