1 files changed, 46 insertions, 11 deletions

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