2 files changed, 51 insertions, 3 deletions

diff --git a/lib/core/c++/index.hpp b/lib/core/c++/index.hpp
new file mode 100644
index 0000000..00e597e
--- /dev/null
+++ b/lib/core/c++/index.hpp
@@ -0,0 +1,30 @@
+#pragma once
+
+#include "common.hpp"
+
+namespace kel {
+namespace lbm {
+
+template<typename T, uint64_t D>
+saw::data<sch::FixedArray<sch::UInt64,D>> lower_index_from_pos(const saw::data<sch::Vector<T,D>>& pos){
+	saw::data<sch::FixedArray<sch::UInt64,D>> ind;
+
+	for(saw::data<sch::UInt64> i{0u}; i < ind.size(); ++i){
+		ind.at(i).set(std::max(pos.at(i).template cast_to<sch::UInt64>().get(),0));
+	}
+
+	return ind;
+}
+
+template<typename T, uint64_t D>
+saw::data<sch::FixedArray<sch::UInt64,D>> upper_index_from_pos(const saw::data<sch::Vector<T,D>>& pos){
+	auto ind = lower_index_from_pos(pos);
+
+	for(saw::data<sch::UInt64> i{0u}; i < ind.size(); ++i){
+		++ind.at(i);
+	}
+	return ind;
+}
+
+}
+}
diff --git a/lib/core/c++/particle/particle.hpp b/lib/core/c++/particle/particle.hpp
index 9bf310e..8509df6 100644
--- a/lib/core/c++/particle/particle.hpp
+++ b/lib/core/c++/particle/particle.hpp
@@ -60,13 +60,15 @@ saw::data<sch::Particle<T,D>, sch::ParticleCollisionSpheroid<T>> create_spheroid
 		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>> 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">();
 
@@ -77,10 +79,26 @@ saw::data<sch::Particle<T,D>, sch::ParticleCollisionSpheroid<T>> create_spheroid
 	auto& rad = coll.template get<"radius">();
 
 	pos = pos_p;
-	pos_old = pos - vec_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;
 }