Doing aabb computations

2 files changed, 95 insertions, 11 deletions

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;
 	}