9 files changed, 694 insertions, 0 deletions
diff --git a/modules/core/c++/particle/aabb.hpp b/modules/core/c++/particle/aabb.hpp
new file mode 100644
index 0000000..1773dea
--- /dev/null
+++ b/modules/core/c++/particle/aabb.hpp
@@ -0,0 +1,55 @@
+#pragma once
+
+#include "common.hpp"
+#include "schema.hpp"
+
+namespace kel {
+namespace lbm {
+template<typename PGroup>
+class particle_aabb final {
+	static_assert(saw::always_false<PGroup>, "Not supported");
+};
+
+template<typename T, uint64_t D>
+class particle_aabb<
+	sch::ParticleGroup<T,D,coll::Spheroid<T>>
+> final {
+public:
+	using Schema = sch::ParticleGroup<T,D,coll::Spheroid<T>>;
+
+	using AABB = sch::Struct<
+		sch::Member<sch::FixedArray<sch::UInt64,D>,"a">,
+		sch::Member<sch::FixedArray<sch::UInt64,D>,"b">
+	>;
+public:
+	template<typename Encode>
+	static constexpr saw::data<AABB> calculate(const saw::data<Schema,Encode>& p_grp, const saw::data<sch::FixedArray<sch::UInt64,1u>>& i, const saw::data<sch::FixedArray<sch::UInt64,D>>& meta){
+
+		saw::data<AABB> aabb;
+		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">();
+
+		auto& a = aabb.template get<"a">();
+		auto& b = aabb.template get<"b">();
+
+		const saw::data<sch::Scalar<T>>& rad_d = p_grp.template get<"collision">().template get<"radius">().at({0u});
+
+		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/modules/core/c++/particle/blur.hpp b/modules/core/c++/particle/blur.hpp
new file mode 100644
index 0000000..b7a1988
--- /dev/null
+++ b/modules/core/c++/particle/blur.hpp
@@ -0,0 +1,37 @@
+#pragma once
+
+namespace kel {
+namespace lbm {
+template<typename T, uint64_t D>
+void blur_mask(saw::data<sch::Array<T,D>>& p_mask){
+
+	saw::data<T> mid{2.0/3.0};
+	saw::data<T> edge{1.0/6.0};
+
+	// saw::data<sch::FixedArray<sch::UInt64>,2u> blur_weights{{2.0/3.0},{1.0/6.0}};
+
+	auto meta = p_mask.dims();
+	saw::data<sch::Array<T,D>> blurred_mask{meta};
+
+	/* 1D blur into N-D Blur*/
+	for(saw::data<sch::UInt64> i{0u}; i < saw::data<sch::UInt64>{D}; ++i){
+		iterator<D>::apply([&](const auto& index){
+			blurred_mask.at(index) = p_mask.at(index) * mid;
+
+			if(index.at({i}).get() > 0u){
+				auto l_ind = index;
+				l_ind.at({i}) = ind.at({i}) - 1u;
+				blurred_mask.at(index) = blurred_mask.at(index) + p_mask.at(l_ind) * edge;
+			}
+			if((index.at({i}).get() + 1u) < meta.at({i})){
+				auto r_ind = index;
+				r_ind.at({i}) = ind.at({i}) + 1u;
+				blurred_mask.at(index) = blurred_mask.at(index) + p_mask.at(r_ind) * edge;
+			}
+		},{},meta);
+
+		p_mask = blurred_mask;
+	}
+}
+}
+}
diff --git a/modules/core/c++/particle/common.hpp b/modules/core/c++/particle/common.hpp
new file mode 100644
index 0000000..9e673c2
--- /dev/null
+++ b/modules/core/c++/particle/common.hpp
@@ -0,0 +1,3 @@
+#pragma once
+
+#include "../common.hpp"
diff --git a/modules/core/c++/particle/geometry/circle.hpp b/modules/core/c++/particle/geometry/circle.hpp
new file mode 100644
index 0000000..cf9b87e
--- /dev/null
+++ b/modules/core/c++/particle/geometry/circle.hpp
@@ -0,0 +1,85 @@
+#pragma once
+
+#include "../particle.hpp"
+
+namespace kel {
+namespace lbm {
+
+template<typename T, uint64_t D>
+class particle_circle_geometry {
+private:
+public:
+	particle_circle_geometry()
+	{}
+
+	template<typename MT = T>
+	saw::data<sch::ParticleMask<MT,D>> generate_mask(uint64_t resolution, uint64_t boundary_nodes = 0) const {
+		saw::data<sch::ParticleMask<MT,D>> mask;
+
+		auto& grid = mask.template get<"grid">();
+		auto& com = mask.template get<"center_of_mass">();
+		com = {};
+
+		//uint64_t rad_i = static_cast<uint64_t>(resolution * radius_.get())+1u;
+		uint64_t diameter_i = resolution;
+		uint64_t size = diameter_i + 2*boundary_nodes;
+		grid = {{{size,size}}};
+
+		saw::data<T> delta_x{static_cast<typename saw::native_data_type<T>::type>(2.0  / static_cast<double>(diameter_i))};
+		saw::data<T> center = (saw::data<T>{1.0} + saw::data<T>{2.0} * saw::data<T>{static_cast<saw::native_data_type<T>::type>(boundary_nodes)/diameter_i});
+
+		saw::data<sch::FixedArray<sch::UInt64,D>> boundary_arr;
+		for(uint64_t i = 0u; i < D; ++i){
+			boundary_arr.set(boundary_nodes);
+		}
+
+		iterator<D>::apply([&](const auto& index){
+			grid.at(index).set(0);
+		}, {}, grid.dims());
+		
+		iterator<D>::apply([&](const auto& index){
+			saw::data<sch::Vector<T,D>> f_ind;
+			for(uint64_t i = 0u; i < D; ++i){
+				f_ind.at({{i}}) = (index.at({{i}}) + 0.5) * delta_x - center;
+			}
+
+			auto norm_f_ind = saw::math::norm(f_ind);
+		}, {}, grid.dims(),boundary_arr);
+
+		for(uint64_t i = 0; i < size; ++i){
+			for(uint64_t j = 0; j < size; ++j){
+				grid.at({{i,j}}).set(0);
+				if(i >= boundary_nodes and j >= boundary_nodes and i < (diameter_i + boundary_nodes) and j < (diameter_i + boundary_nodes) ){
+					saw::data<T> fi = saw::data<T>{static_cast<saw::native_data_type<T>::type>(0.5+i)} * delta_x - center;
+					saw::data<T> fj = saw::data<T>{static_cast<saw::native_data_type<T>::type>(0.5+j)} * delta_x - center;
+
+					auto norm_f_ij = fi*fi + fj*fj;
+					if(norm_f_ij.get() <= 1){
+						grid.at({{i,j}}).set(1);
+					}
+				}
+			}
+		}
+
+		// I don't fully remember what this did
+		saw::data<T> total_mass{};
+		iterator::apply<D>::apply([&](const saw::data<sch::FixedArray<sch::UInt64,D>>& index){
+			auto ind_vec = saw::math::vectorize_data(index).template cast_to<T>();
+			for(uint64_t i{0u}; i < D; ++i){
+				ind_vec.at({{i}}) = ind_vec.at({{i}}) * grid.at(index);
+			}
+			com = com + ind_vec;
+
+			total_mass = total_mass + grid.at(index);
+		},{}, grid.dims());
+
+		for(uint64_t i{0u}; i < D; ++i){
+			com.at({{i}}) = com.at({{i}}) / total_mass;
+		}
+
+		return mask;
+	}
+};
+
+}
+}
diff --git a/modules/core/c++/particle/geometry/cube.hpp b/modules/core/c++/particle/geometry/cube.hpp
new file mode 100644
index 0000000..6392de8
--- /dev/null
+++ b/modules/core/c++/particle/geometry/cube.hpp
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "../particle.hpp"
+
+namespace kel {
+namespace lbm {
+template<typename T, uint64_t D>
+class particle_cubic_geometry {
+private:
+public:
+	template<typename MT = T>
+	saw::data<sch::ParticleMask<MT,D>> generate_mask(uint64_t resolution, uint64_t bound_nodes = 0u){
+
+		saw::data<sch::ParticleMask<MT,D>> mask;
+
+		auto& grid = mask.template get<"grid">();
+		auto& com = mask.template get<"center_of_mass">();
+
+
+	}
+}
+}
diff --git a/modules/core/c++/particle/particle.hpp b/modules/core/c++/particle/particle.hpp
new file mode 100644
index 0000000..8e75e5a
--- /dev/null
+++ b/modules/core/c++/particle/particle.hpp
@@ -0,0 +1,246 @@
+#pragma once
+
+#include <forstio/codec/math.hpp>
+#include <forstio/codec/data_math.hpp>
+#include <forstio/codec/data.hpp>
+
+#include "../iterator.hpp"
+
+#include "schema.hpp"
+#include "aabb.hpp"
+#include "particle_opa.hpp"
+
+namespace kel {
+namespace lbm {
+
+template<typename T, uint64_t D>
+saw::data<sch::ParticleGroup<T,D, coll::Spheroid<T>>> create_spheroid_particle_group(
+		saw::data<sch::Scalar<T>> radius_p,
+		saw::data<sch::Scalar<T>> density_p,
+		const saw::data<sch::UInt64>& mask_resolution
+){
+	saw::data<sch::ParticleGroup<T,D,coll::Spheroid<T>>> part;
+
+	auto& rad_s = part.template get<"collision">().at({0u}).template get<"radius">();
+	rad_s = radius_p;
+
+	auto& mask = part.template get<"mask">();
+	auto& density = part.template get<"density">().at({{0u}});
+
+	auto& total_mass = part.template get<"total_mass">().at({{0u}});
+	// Paranoia
+	total_mass.at({}) = {};
+
+	static_assert(D >= 1u and D <= 3u, "Dimensions only supported for Dim 1,2 & 3.");
+	density = density_p;
+
+	saw::data<sch::FixedArray<sch::UInt64,D>> mask_dims;
+	for(uint64_t i = 0u; i < D; ++i){
+		mask_dims.at({i}) = mask_resolution;
+	}
+	saw::data<T> rad_d = radius_p.at({});
+	saw::data<T> dia_d = rad_d * 2;
+
+	mask = {mask_dims};
+
+	auto& mask_step = part.template get<"mask_step">().at({{0u}});
+	mask_step.at({}) = dia_d / mask_resolution.template cast_to<T>();
+
+	auto& com = part.template get<"center_of_mass">().at({{0u}});
+	// Paranoia
+	for(uint64_t i = 0u; i < D; ++i){
+		com.at({{i}}) = {};
+	}
+	saw::data<sch::UInt64> ele_ctr{0u};
+
+	// Radius ^ 2
+	saw::data<sch::Scalar<T>> rad_2_d;
+	rad_2_d.at({}) = rad_d * rad_d;
+
+	saw::data<sch::Vector<T,D>> center;
+	for(uint64_t i = 0u; i < D; ++i){
+		center.at({{i}}) = rad_d;
+	}
+
+	iterator<D>::apply([&](const auto& index){
+		++ele_ctr;
+
+		saw::data<sch::Vector<T,D>> offset_index = saw::math::vectorize_data(index).template cast_to<T>() - center;
+
+		auto& dpi = mask.at(index);
+
+		for(uint64_t i = 0u; i < D; ++i){
+			com.at({{i}}) = com.at({{i}}) + index.at({i}).template cast_to<T>() * dpi;
+		}
+
+		total_mass.at({}) = total_mass.at({}) + dpi;
+
+	},{},mask_dims);
+
+	for(uint64_t i = 0u; i < D; ++i){
+		com.at({{i}}) = com.at({{i}}) / total_mass.at({});
+	}
+	return part;
+}
+
+/*
+template<typename T, uint64_t D>
+saw::data<sch::Particle<T,D, sch::ParticleCollisionSpheroid<T>>> create_spheroid_particle(
+		saw::data<sch::Vector<T,D>> pos_p,
+		saw::data<sch::Vector<T,D>> vec_p,
+		saw::data<sch::Vector<T,D>> acc_p,
+		saw::data<sch::Vector<T,D>> rot_pos_p,
+		saw::data<sch::Vector<T,D>> rot_vel_p,
+		saw::data<sch::Vector<T,D>> rot_acc_p,
+		saw::data<sch::Scalar<T>> rad_p,
+		saw::data<sch::Scalar<T>> density_p,
+		saw::data<sch::Scalar<T>> dt
+	){
+
+	saw::data<sch::Particle<T,D>> part;
+	auto& body = part.template get<"rigid_body">();
+	auto& mass = part.template get<"mass">();
+	
+	auto& pos = body.template get<"position">();
+	auto& pos_old = body.template get<"position_old">();
+	auto& acc = body.template get<"acceleration">();
+
+	auto& rot = body.template get<"rotation">();
+	auto& rot_old = body.template get<"rotation_old">();
+
+	auto& coll = part.template get<"collision">();
+	auto& rad = coll.template get<"radius">();
+
+	pos = pos_p;
+	pos_old = pos - vec_p * dt;
+	acc = acc_p;
+	rad = rad_p;
+
+	if constexpr ( D == 1u){
+		saw::data<sch::Scalar<T>> c;
+		c.at({}).set(2.0);
+		mass = rad_p * c * density_p;
+	} else if constexpr ( D == 2u){
+		saw::data<sch::Scalar<T>> pi;
+		pi.at({}).set(3.14159);
+		mass = rad_p * rad_p * pi * density_p;
+	} else if constexpr ( D == 3u ){
+		saw::data<sch::Scalar<T>> c;
+		c.at({}).set(3.14159 * 4.0 / 3.0);
+		mass = rad_p * rad_p * rad_p * c * density_p;
+	} else {
+		static_assert(D == 0u or D > 3u, "Dimensions only supported for Dim 1,2 & 3.");
+	}
+
+	return part;
+}
+*/
+
+template<typename T,uint64_t D>
+constexpr auto verlet_step_lambda = [](saw::data<sch::Particle<T,D>>& particle, saw::data<sch::Scalar<T>> time_step_delta){
+	auto& body = particle.template get<"rigid_body">();
+
+	auto& pos = body.template get<"position">();
+	auto& pos_old = body.template get<"position_old">();
+
+	auto& pos_acc = body.template get<"acceleration">();
+	
+	auto& rot = body.template get<"rotation">();
+	auto& rot_old = body.template get<"rotation_old">();
+
+	auto& rot_acc = body.template get<"angular_acceleration">();
+
+	auto tsd_squared = time_step_delta * time_step_delta;
+
+	saw::data<sch::Vector<T,D>> pos_new;
+	// Actual step
+	saw::data<sch::Scalar<T>> two;
+	two.at({}).set(2.0);
+	pos_new = pos * two - pos_old + pos_acc * tsd_squared;
+
+	// Angular
+	saw::data<typename sch::impl::rotation_type_helper<T,D>::Schema> rot_new;
+	rot_new = rot * two - rot_old + rot_acc * tsd_squared;
+
+	// Swap - Could be std::swap?
+	pos_old = pos;
+	pos = pos_new;
+
+	rot_old = rot;
+	rot = rot_new;
+};
+
+template<typename T, uint64_t D>
+constexpr auto handle_collision = [](
+		saw::data<sch::Particle<T,D>>& left, const saw::data<sch::Scalar<T>>& mass_l,
+		saw::data<sch::Particle<T,D>>& right, const saw::data<sch::Scalar<T>>& mass_r,
+		saw::data<sch::Vector<T,D>> unit_pos_rel, saw::data<sch::Vector<T,D>> vel_rel,
+		saw::data<sch::Scalar<T>> d_t
+){
+	auto& rb_l = left.template get<"rigid_body">();
+	auto& pos_l = rb_l.template get<"position">();
+	auto& pos_old_l = rb_l.template get<"position_old">();
+	auto vel_l = (pos_l-pos_old_l)/d_t;
+
+	auto& rb_r = right.template get<"rigid_body">();
+	auto& pos_r = rb_r.template get<"position">();
+	auto& pos_old_r = rb_r.template get<"position_old">();
+	auto vel_r = (pos_r-pos_old_r)/d_t;
+
+	auto vel_pos_rel_dot = saw::math::dot(unit_pos_rel,vel_rel);
+
+  if( vel_pos_rel_dot.at({{0u}}).get() < 0.0 ){
+		pos_l = pos_l + vel_rel * unit_pos_rel * d_t;
+		pos_r = pos_r - vel_rel * unit_pos_rel * d_t;
+	}
+};
+
+
+template<typename T, uint64_t D, typename Collision>
+constexpr auto broadphase_collision_distance_squared = [](
+	saw::data<sch::Particle<T,D>>& left,
+	const saw::data<Collision>& coll_l,
+	saw::data<sch::Particle<T,D>>& right,
+	const saw::data<Collision>& coll_r
+) -> std::pair<bool,saw::data<sch::Scalar<T>>>{
+
+	auto rad_l = coll_l.template get<"radius">();
+	auto rad_r = coll_r.template get<"radius">();
+
+	auto& rb_l = left.template get<"rigid_body">();
+	auto& rb_r = right.template get<"rigid_body">();
+
+	auto& pos_l = rb_l.template get<"position">();
+	auto& pos_r = rb_r.template get<"position">();
+
+	auto pos_dist = pos_l - pos_r;
+
+	auto norm_2 = saw::math::dot(pos_dist,pos_dist);
+
+	saw::data<sch::Scalar<T>> two;
+	two.at({}) = 2.0;
+	auto rad_ab_2 = rad_l * rad_l + rad_r * rad_r + rad_r * rad_l * two;
+
+	return std::make_pair((norm_2.at({}).get() < rad_ab_2.at({}).get()), norm_2);
+};
+/**
+*
+*
+*/
+template<typename T, uint64_t D, typename Collision>
+constexpr auto broadphase_collision_check = [](
+	saw::data<sch::Particle<T,D>>& left,
+	const saw::data<Collision>& coll_l,
+	saw::data<sch::Particle<T,D>>& right,
+	const saw::data<Collision>& coll_r
+) -> bool{
+	return broadphase_collision_distance_squared<T,D,Collision>(left,coll_l,right,coll_r).first;
+};
+
+
+
+namespace impl {
+}
+
+}
+}
diff --git a/modules/core/c++/particle/particle_opa.hpp b/modules/core/c++/particle/particle_opa.hpp
new file mode 100644
index 0000000..a5e6c28
--- /dev/null
+++ b/modules/core/c++/particle/particle_opa.hpp
@@ -0,0 +1,49 @@
+#pragma once
+
+#include "../component.hpp"
+#include "common.hpp"
+#include "schema.hpp"
+#include "porosity.hpp"
+
+namespace kel {
+namespace lbm {
+namespace cmpt {
+struct OneParticleAt {};
+}
+
+template<typename T, typename Descriptor, typename Encode>
+
+class component<T,Descriptor,cmpt::OneParticleAt, Encode> final {
+private:
+	saw::data<sch::Vector<T,Descriptor::D>> pos_;
+	saw::data<sch::Scalar<T>> rad_;
+	saw::data<sch::Scalar<T>> eps_;
+public:
+	component(
+		const saw::data<sch::Vector<T,Descriptor::D>> pos__,
+		const saw::data<sch::Scalar<T>> rad__,
+		const saw::data<sch::Scalar<T>> eps__
+	):
+		pos_{pos__},
+		rad_{rad__},
+		eps_{eps__}
+	{}
+
+	template<typename MacroFieldSchema>
+	void apply(const saw::data<MacroFieldSchema, Encode>& macros, const saw::data<sch::FixedArray<sch::UInt64,Descriptor::D>> index, saw::data<sch::UInt64> time_step) const {
+		using dfi = df_info<T,Descriptor>;
+
+		auto& porous_f = macros.template get<"porosity">();
+
+		auto& porous = porous_f.at(index);
+
+		auto pos_ind = saw::math::vectorize_data(index);
+
+		// 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/modules/core/c++/particle/porosity.hpp b/modules/core/c++/particle/porosity.hpp
new file mode 100644
index 0000000..2f39d2e
--- /dev/null
+++ b/modules/core/c++/particle/porosity.hpp
@@ -0,0 +1,129 @@
+#pragma once
+
+#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:
+
+	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">();
+		auto& part_i = particles.at({p_i});
+
+		auto& part_i_rb = part_i.template get<"rigid_body">();
+		auto& pirb = part_i_rb.template get<"position">();
+
+		auto dist = lbm_pos - pirb;
+
+		// index 0 is at 
+
+		return {};
+	}
+};
+
+template<typename T, uint64_t D>
+class particle_porosity<T, D, por::ParticleSpheroid<T>> final {
+public:
+	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(0);
+			return por;
+		}
+		if(s_dist.at({}) >= rad_high.at({})){
+			por.at({}).set(1);
+			return por;
+		}
+
+		{
+			typename saw::native_data_type<T>::type inner = (std::numbers::pi / ( 2 * eps.at({}).get() )) * (s_dist - rad_low).at({}).get();
+			auto cos_inner = std::sin(inner);
+			auto cos_i_2 = cos_inner * cos_inner;
+			por.at({}).set(cos_i_2);
+		}
+
+		return por;
+	}
+};
+
+
+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;
+
+		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 = saw::math::dot(dist,dist);
+		dist_len.at({}).set(std::sqrt(dist_len.at({}).get()));
+
+		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>> 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;
+		// saw::data<sch::Scalar<T>> rad_2;
+		// rad_2 = rad_d * rad_d;
+
+		saw::data<sch::Scalar<T>> inside;
+		if(dist_len.at({}) <= rad_d_eps_n.at({})){
+			inside.at({}).set(0);
+			return inside;
+		}
+		if(dist_len.at({}) > rad_d_eps_p.at({})){
+			inside.at({}).set(1);
+			return inside;
+		}
+
+		/*
+		 * cos^2 ( ||x-X(t)||_2 - (R-eps/2) )
+		 */
+		{
+			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);
+		}
+		return inside;
+	}
+};
+
+}
+}
diff --git a/modules/core/c++/particle/schema.hpp b/modules/core/c++/particle/schema.hpp
new file mode 100644
index 0000000..714a16f
--- /dev/null
+++ b/modules/core/c++/particle/schema.hpp
@@ -0,0 +1,68 @@
+#pragma once
+
+#include "common.hpp"
+
+namespace kel {
+namespace lbm {
+
+namespace coll {
+template<typename T>
+struct Spheroid {
+	using ValueSchema = T;
+	using Schema = sch::Struct<
+		sch::Member<sch::Scalar<ValueSchema>,"radius">
+	>;
+};
+}
+
+namespace sch {
+using namespace saw::schema;
+
+namespace impl {
+template<typename T,uint64_t D>
+struct rotation_type_helper;
+
+template<typename T>
+struct rotation_type_helper<T,2u> {
+	using Schema = Scalar<T>;
+};
+
+template<typename T>
+struct rotation_type_helper<T,3u> {
+	using Schema = Vector<T,3u>;
+};
+}
+
+template<typename T, uint64_t D>
+using ParticleRigidBody = Struct<
+	Member<Vector<T,D>, "position">,
+	Member<Vector<T,D>, "position_old">,
+	Member<typename impl::rotation_type_helper<T,D>::Schema, "rotation">,
+	Member<typename impl::rotation_type_helper<T,D>::Schema, "rotation_old">,
+
+	Member<Vector<T,D>, "acceleration">,
+	Member<typename impl::rotation_type_helper<T,D>::Schema, "angular_acceleration">
+>;
+
+
+template<typename T, uint64_t D>
+using Particle = Struct<
+	Member<ParticleRigidBody<T,D>, "rigid_body">
+	// Problem is that dynamic data would two layered
+	// Member<Array<Float64,D>, "mask">,
+>;
+
+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">,
+	Member<FixedArray<Scalar<T>,1u>, "total_mass">,
+	Member<Array<Particle<T,D>,1u>, "particles">
+>;
+}
+}
+}