3 files changed, 66 insertions, 29 deletions
diff --git a/lib/core/c++/particle/particle_opa.hpp b/lib/core/c++/particle/particle_opa.hpp
index 4588a55..a5e6c28 100644
--- a/lib/core/c++/particle/particle_opa.hpp
+++ b/lib/core/c++/particle/particle_opa.hpp
@@ -1,7 +1,9 @@
 #pragma once
 
-#include "common.hpp"
 #include "../component.hpp"
+#include "common.hpp"
+#include "schema.hpp"
+#include "porosity.hpp"
 
 namespace kel {
 namespace lbm {
@@ -35,11 +37,12 @@ public:
 
 		auto& porous = porous_f.at(index);
 
-
 		auto pos_ind = saw::math::vectorize_data(index);
 
-		auto diff = pos_ind - pos_;
+		// Write out value
+		auto diff = pos_ind.template cast_to<T>() - pos_;
 		auto diff_dot = saw::math::dot(diff,diff);
+		porous = particle_porosity<T,Descriptor::D,por::ParticleSpheroid<T>>::calculate(diff, rad_, eps_);
 	}
 };
 }
diff --git a/lib/core/c++/particle/porosity.hpp b/lib/core/c++/particle/porosity.hpp
index f555cae..11185c5 100644
--- a/lib/core/c++/particle/porosity.hpp
+++ b/lib/core/c++/particle/porosity.hpp
@@ -1,16 +1,21 @@
 #pragma once
 
-#include "particle.hpp"
+#include "common.hpp"
+#include "schema.hpp"
 #include "../math/n_closest.hpp"
 
 namespace kel {
 namespace lbm {
+namespace por {
+template<typename T>
+struct ParticleSpheroid {};
+}
+
 template<typename T, uint64_t D, typename Coll>
 class particle_porosity {
 public:
 
-	
-	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){
+	static 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">();
@@ -27,20 +32,47 @@ public:
 	}
 };
 
-
-
-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 {
+template<typename T, uint64_t D>
+class particle_porosity<T, D, por::ParticleSpheroid<T>> final {
 public:
-	saw::data<sch::Scalar<T>> calculate(const saw::data<sch::Vector<T,D>>& lbm_pos, saw::data<sch::Scalar<T>> rad) const {
-		saw::data<sch::Scalar<T>> pos;
-		
+	static saw::data<sch::Scalar<T>> calculate(const saw::data<sch::Vector<T,D>>& lbm_rel_dist, saw::data<sch::Scalar<T>> rad, saw::data<sch::Scalar<T>> eps){
+		saw::data<sch::Scalar<T>> por;
 		
+		auto s_dist_2 = saw::math::dot(lbm_rel_dist,lbm_rel_dist);
+		auto s_dist = saw::math::sqrt(s_dist_2);
+
+		saw::data<sch::Scalar<T>> eps_h;
+		eps_h.at({}) = eps.at({}).get() / 2;
+
+		auto rad_low = rad - eps_h;
+		auto rad_high = rad + eps_h;
+
+		if(s_dist.at({}) <= rad_low.at({})){
+			por.at({}).set(1);
+			return por;
+		}
+		if(s_dist.at({}) >= rad_high.at({})){
+			por.at({}).set(0);
+			return por;
+		}
+
+		{
+			typename saw::native_data_type<T>::type inner = (std::numbers::pi / ( eps.at({}).get() )) * (s_dist - rad_low).at({}).get();
+			auto cos_inner = std::cos(inner);
+			auto cos_i_2 = cos_inner * cos_inner;
+			por.at({}).set(cos_i_2);
+		}
+
+		return por;
 	}
+};
 
-	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) const {
+
+template<typename T, uint64_t D>
+class particle_porosity<T, D, coll::Spheroid<T>> final {
+public:
+	static saw::data<sch::Scalar<T>> calculate(const saw::data<sch::ParticleGroup<T,D,coll::Spheroid<T> > >& part_group, uint64_t i, const saw::data<sch::Vector<T,D>>& lbm_pos) {
 		saw::data<sch::Scalar<T>> por;
-		por.at({});
 
 		auto& parts = part_group.template get<"particles">();
 		auto& pi = parts.at({i});
@@ -51,18 +83,19 @@ public:
 		// 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& 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>> dist_len = saw::math::dot(dist,dist);
+		dist_len.at({}).set(std::sqrt(dist_len.at({}).get()));
 
-		saw::data<sch::Scalar<T>> rad_d{radius};
-		saw::data<sch::Scalar<T>> eps_half{eps *0.5};
+		auto& coll = part_group.template get<"collision">();
+		const saw::data<sch::Scalar<T>>& rad_d = coll.template get<"radius">();
+		const auto& eps = part_group.template get<"epsilon">();
+		// Move this somewhere
 		
-		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>> eps_h;
+		eps_h.at({}) = eps.at({}).get() / 2;
+
+		saw::data<sch::Scalar<T>> rad_d_eps_p = rad_d + eps_h;
+		saw::data<sch::Scalar<T>> rad_d_eps_n = rad_d - eps_h;
 
 		// 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;
@@ -71,19 +104,19 @@ public:
 
 		saw::data<sch::Scalar<T>> inside;
 		if(dist_len.at({}) <= rad_d_eps_n.at({})){
-			inside.at({}).set(1);
+			inside.at({}).set(0);
 			return inside;
 		}
 		if(dist_len.at({}) > rad_d_eps_p.at({})){
-			inside.at({}).set(0);
-			return inside
+			inside.at({}).set(1);
+			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();
+			typename saw::native_data_type<T>::type inner = (std::numbers::pi / (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);
diff --git a/lib/core/c++/particle/schema.hpp b/lib/core/c++/particle/schema.hpp
index 18a697a..714a16f 100644
--- a/lib/core/c++/particle/schema.hpp
+++ b/lib/core/c++/particle/schema.hpp
@@ -56,6 +56,7 @@ template<typename T, uint64_t D, typename CollisionType = coll::Spheroid<T>>
 using ParticleGroup = Struct<
 	Member<Array<T,D>, "mask">,
 	Member<FixedArray<typename CollisionType::Schema,1u>, "collision">,
+	Member<FixedArray<Scalar<T>,1u>, "epsilon">,
 	Member<FixedArray<Scalar<T>,1u>, "mask_step">,
 	Member<FixedArray<Scalar<T>,1u>, "density">,
 	Member<FixedArray<Vector<T,D>,1u>, "center_of_mass">,