Doing aabb computations

4 files changed, 139 insertions, 15 deletions

diff --git a/lib/core/c++/chunk.hpp b/lib/core/c++/chunk.hpp
index 0f92437..635af91 100644
--- a/lib/core/c++/chunk.hpp
+++ b/lib/core/c++/chunk.hpp
@@ -77,6 +77,11 @@ public:
 		return values_.at(ind);
 	}
 
+	/*
+	const data<ValueSchema, Encode>& neighbour_at(const data<schema::FixedArray<schema::UInt64,sizeof...(Sides)>>& index) const {
+	}
+	*/
+
 	static constexpr auto get_dims(){
 		return data<schema::FixedArray<schema::UInt64, sizeof...(Sides)>,Encode>{{Sides...}};
 	}
diff --git a/lib/core/c++/hlbm.hpp b/lib/core/c++/hlbm.hpp
index 7590cc2..ed49353 100644
--- a/lib/core/c++/hlbm.hpp
+++ b/lib/core/c++/hlbm.hpp
@@ -96,20 +96,55 @@ public:
 	}
 };
 
-template<typename T, typename Descriptor, typename Encode>
+namespace impl {
+
+}
+
+template<typename T, typename Desc, typename Encode>
 class component<T, Descriptor, cmpt::HlbmParticle, Encode> final {
 private:
-	typename saw::native_data_type<T>::type relaxation_;
-	saw::data<T> frequency_;
+	template<typename CellFieldSchema, typename MacroFieldSchema, typename ParticleSchema, uint64_t i>
+	void apply_i(const saw::data<CellFieldSchema, Encode>& field, const saw::data<MacroFieldSchema,Encode>& macros, const saw::data<ParticleSchema,Encode>& part_groups, saw::data<sch::FixedArray<sch::UInt64,1u>> index, saw::data<sch::UInt64> time_step) const {
+		
+	}	
 public:
+	/*
+	template<typename CellFieldSchema, typename MacroFieldSchema, typename ParticleSchema, uint64_t i>
+	void apply_i()
+	*/
 
 	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>& part_groups, saw::data<sch::FixedArray<sch::UInt64,1u>> index, saw::data<sch::UInt64> time_step) const {
 		/// Figure out how to access the particle list
 		// auto& p = particles.at(i);
 
 		/// Iterate over the grid bounds
 		// auto& grid = p.template get<"grid">();
+
+		auto& part_spheroid_group = part_groups.template get<0>();
+		{
+			auto& parts = part_spheroid_group.template get<"particles">();
+			auto parts_size = parts.size();
+
+			auto& pi = parts.at(index);
+			auto& pirb = pi.template get<"rigid_body">();
+			auto& pirb_pos = pirb.template get<"position">();
+
+			saw::data<sch::FixedArray<sch::UInt64,Desc::D>> start;
+			saw::data<sch::FixedArray<sch::UInt64,Desc::D>> stop;
+
+			/// Ok, I iterate over the space which covers our particle? So lower bounds to upper bounds
+			for(uint64_t i{0u}; i < Desc::D; ++i){
+				
+			}
+
+			iterator<Desc::D>::apply([&](const auto& index){
+				// ask for the d_k value here.
+				// For every value im iterating over I need sth
+			},start,stop);
+
+			// Check
+		}
 		
 
 	}
diff --git a/lib/core/c++/particle/aabb.hpp b/lib/core/c++/particle/aabb.hpp
new file mode 100644
index 0000000..aec95ca
--- /dev/null
+++ b/lib/core/c++/particle/aabb.hpp
@@ -0,0 +1,49 @@
+#pragma once
+
+#include "particle.hpp"
+
+namespace kel {
+namespace lbm {
+template<typename T, uint64_t D, typename PColl>
+class particle_aabb final {
+};
+
+template<typename T, uint64_t D, typename saw::native_data_type<T>::type radius>
+class particle_aabb<ParticleGroup<T,D,sch::ParticleCollisionSpheroid<T,radius> > > final {
+public:
+	using Schema = sch::ParticleGroup<T,D,sch::ParticleCollisionSpheroid<T,radius>>;
+
+	using AABB = Struct<
+		Member<sch::FixedArray<sch::UInt64,D>"a">,
+		Member<sch::FixedArray<sch::UInt64,D>"b">
+	>;
+public:
+	static constexpr saw::data<AABB> get(const saw::data<Schema>& p_grp, const saw::data<sch::FixedArray<sch::UInt64,1u>>& i, const saw::data<sch::FixedArray<sch::UInt64,D>>& meta){
+		auto& parts = p_grp.template get<"particles">();
+
+		auto& pi = parts.at(i);
+		auto& pirb = pi.template get<"rigid_body">();
+		auto& pirb_pos = pirb.template get<"position">();
+
+		saw::data<AABB> aabb;
+		auto& a = aabb.template get<"a">();
+		auto& b = aabb.template get<"b">();
+
+		saw::data<sch::Scalar<T>> rad_d;
+		rad_d.at({}).set(radius);
+
+		saw::data<sch::Vector<T,D>> lower;
+		saw::data<sch::Vector<T,D>> upper;
+
+		for(uint64_t i{0u}; i < D; ++i){
+			lower.at({{i}}) = pirb_pos.at({{i}}) >= rad_d.at({}) ? (pirb_pos.at({{i}}) - rad_d.at({})) : saw::data<T>{0};
+			a.at({i}) = lower.at({{i}}).template cast_to<sch::UInt64>();
+			upper.at({{i}}) = pirb_pos.at({{i}}) + rad_d.at({});
+			b.at({i}) = (upper.at({{i}})+saw::data<T>{1}).template cast_to<sch::UInt64>()
+		}
+		
+		return aabb;
+	}
+};
+}
+}
diff --git a/lib/core/c++/particle/porosity.hpp b/lib/core/c++/particle/porosity.hpp
index aa1ce5b..39d9652 100644
--- a/lib/core/c++/particle/porosity.hpp
+++ b/lib/core/c++/particle/porosity.hpp
@@ -8,7 +8,9 @@ namespace lbm {
 template<typename T, uint64_t D, typename Coll>
 class particle_porosity {
 public:
-	saw::data<sch::Scalar<T>> calculate(const saw::data<>& part_group, uint64_t p_i, const saw::data<sch::Vector<T,D>>& lbm_pos){
+
+	
+	saw::data<sch::Scalar<T>> calculate(const saw::data<sch::ParticleGroup<T,D,Coll>>& part_group, uint64_t p_i, const saw::data<sch::Vector<T,D>>& lbm_pos){
 		auto& mask = part_group.template get<"mask">();
 
 		auto& particles = part_group.template get<"particles">();
@@ -17,7 +19,7 @@ public:
 		auto& part_i_rb = part_i.template get<"rigid_body">();
 		auto& pirb = part_i_rb.template get<"position">();
 
-		auto& dist = lbm_pos = lbm_pos - pirb;
+		auto dist = lbm_pos - pirb;
 
 		// index 0 is at 
 
@@ -26,25 +28,58 @@ public:
 };
 
 
-template<typename T, uint64_t D, typename saw::native_data_type<T>::type radius>
-class particle_porosity<T, D, coll::ParticleCollisionSpheroid<T,radius>> final {
+template<typename T, uint64_t D, typename saw::native_data_type<T>::type radius, typename saw::native_data_type<T>::type eps>
+class particle_porosity<T, D, coll::ParticleCollisionSpheroid<T,radius, eps>> final {
 public:
-	saw::data<sch::Scalar<T>> calculate(const saw::data<sch::Particle>&, uint64_t i, const saw::data<sch::Vector<T,D>>& lbm_pos){
+	saw::data<sch::Scalar<T>> calculate(const saw::data<sch::ParticleGroup<T,D,sch::ParticleCollisionSpheroid<T,radius,eps> > >& part_group, uint64_t i, const saw::data<sch::Vector<T,D>>& lbm_pos){
 		saw::data<sch::Scalar<T>> por;
 		por.at({});
 
-		saw::data<sch::Scalar<T>> dps_2;
+		auto& parts = part_group.template get<"particles">();
+		auto& pi = parts.at({i});
+		auto& pi_rb = pi.template get<"rigid_body">();
+
+		auto& pi_rb_pos = pi_rb.template get<"position">();
+
+		// Basically the queried position minus the center of the particle
+		auto dist = lbm_pos - pi_rb_pos;
+
+		saw::data<sch::Scalar<T>> dist_len;
 		for(uint64_t i{0u}; i < D; ++i){
-			auto& dps_i = lbm_pos.at({{i}});
-			dps_2.at({}) = dps_i * dps_i;
+			auto& d_i = dist.at({{i}});
+			dist_len.at({}) = d_i * d_i;
 		}
+		dist_len.at({}).set(std::sqrt(dist_len.at({}).get());
 
-		saw::data<sch::Scalar<T>> rad_2;
-		rad_2.at({}).set(radius*radius);
+		saw::data<sch::Scalar<T>> rad_d{radius};
+		saw::data<sch::Scalar<T>> eps_half{eps *0.5};
+		
+		saw::data<sch::Scalar<T>> rad_d_eps_p = rad_d + eps_half;
+		saw::data<sch::Scalar<T>> rad_d_eps_n = rad_d - eps_half;
+
+		// saw::data<sch::Scalar<T>> rad_d_eps_p_2 = rad_d_eps_p * rad_d_eps_p;
+		// saw::data<sch::Scalar<T>> rad_d_eps_n_2 = rad_d_eps_n * rad_d_eps_n;
+		// saw::data<sch::Scalar<T>> rad_2;
+		// rad_2 = rad_d * rad_d;
 
 		saw::data<sch::Scalar<T>> inside;
-		if(dps_2.at({}).get() < rad_2.at({}).get()){
+		if(dist_len.at({}) <= rad_d_eps_n.at({})){
 			inside.at({}).set(1);
+			return inside;
+		}
+		if(dist_len.at({}) > rad_d_eps_p.at({})){
+			inside.at({}).set(0);
+			return inside
+		}
+
+		/*
+		 * cos^2 ( ||x-X(t)||_2 - (R-eps/2) )
+		 */
+		{
+			typename saw::native_data_type<T>::type inner = (std::pi / ( 2.0 * eps )) * (dist_len - rad_d_eps_n).at({}).get();
+			auto cos_inner = std::cos(inner);
+			auto cos_i_2 = cos_inner * cos_inner;
+			inside.at({}).set(cos_i_2);
 		}
 		return inside;
 	}