From bf498dca2c333bd66775005571ef915cf27c3ee8 Mon Sep 17 00:00:00 2001
From: "Claudius \"keldu\" Holeksa" <mail@keldu.de>
Date: Wed, 14 Jan 2026 18:45:49 +0100
Subject: Got physics working again

---
 examples/particles_gpu/particles_gpu.cpp | 15 ++++---
 lib/core/c++/particle/particle.hpp       | 75 ++++++++++++++++++--------------
 lib/core/tests/particles.cpp             | 12 +++--
 3 files changed, 59 insertions(+), 43 deletions(-)

diff --git a/examples/particles_gpu/particles_gpu.cpp b/examples/particles_gpu/particles_gpu.cpp
index dfd5af3..abbc192 100644
--- a/examples/particles_gpu/particles_gpu.cpp
+++ b/examples/particles_gpu/particles_gpu.cpp
@@ -42,12 +42,15 @@ saw::error_or<void> lbm_main(int argc, char** argv){
 		}
 	}
 
+	saw::data<sch::Scalar<sch::Float32>> time_step;
+	time_step.at({}).set(0.05f);
+
 	for(saw::data<sch::UInt64> dt{0u}; dt < saw::data<sch::UInt64>{32ul}; ++dt){
 		// Do Verlet Step
 		for(saw::data<sch::UInt64> i{0u}; i < particles.size(); ++i){
 			auto& part_i = particles.at(i);
 
-			verlet_step_lambda<sch::Float32,2u>(part_i,{0.05f});
+			verlet_step_lambda<sch::Float32,2u>(part_i,time_step);
 			auto& body_i = part_i.template get<"rigid_body">();
 			auto& acc_i = body_i.template get<"acceleration">();
 
@@ -70,23 +73,23 @@ saw::error_or<void> lbm_main(int argc, char** argv){
 				auto& pos_j = body_j.template get<"position">();
 				auto& pos_old_j = body_j.template get<"position_old">();
 
-				auto res = broadphase_collision_distance<sch::Float32,2u>(part_i, part_j);
+				auto res = broadphase_collision_distance_squared<sch::Float32,2u>(part_i, part_j);
 				if(res.first){
 					//std::cout<<"Collision"<<std::endl;
 					// Do collision
 
-					auto vel_i = pos_i - pos_old_i;
-					auto vel_j = pos_j - pos_old_j;
+					auto vel_i = (pos_i - pos_old_i)/time_step;
+					auto vel_j = (pos_j - pos_old_j)/time_step;
 
 					auto vel_rel = vel_i - vel_j;
 					auto pos_rel = pos_i - pos_j;
 
 					auto vel_pos_rel_dot = saw::math::dot(vel_rel,pos_rel);
 					if(vel_pos_rel_dot.at({}).get() < 0.0){
-
+						auto pos_rel_normed = pos_rel / saw::math::sqrt(res.second);
 					}
 
-					auto vel_ij = saw::math::dot(vel_i,vel_j);
+					// auto vel_ij = saw::math::dot(vel_i,vel_j);
 
 				}
 			}
diff --git a/lib/core/c++/particle/particle.hpp b/lib/core/c++/particle/particle.hpp
index 8a4ce5d..9bf310e 100644
--- a/lib/core/c++/particle/particle.hpp
+++ b/lib/core/c++/particle/particle.hpp
@@ -12,15 +12,30 @@ struct Spheroid{};
 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<T, "rotation">,
-	Member<T, "rotation_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<T, "rotational_acceleration">
+	Member<typename impl::rotation_type_helper<T,D>::Schema, "rotational_acceleration">
 >;
 
 template<typename T>
@@ -44,8 +59,8 @@ saw::data<sch::Particle<T,D>, sch::ParticleCollisionSpheroid<T>> create_spheroid
 		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<T> rad_p,
-		saw::data<T> density_p
+		saw::data<sch::Scalar<T>> rad_p,
+		saw::data<sch::Scalar<T>> density_p
 	){
 
 	saw::data<sch::Particle<T,D>> part;
@@ -70,54 +85,48 @@ saw::data<sch::Particle<T,D>, sch::ParticleCollisionSpheroid<T>> create_spheroid
 }
 
 template<typename T,uint64_t D>
-constexpr auto verlet_step_lambda = [](saw::data<sch::Particle<T,D>>& particle, saw::data<T> time_step_delta){
-		auto& body = particle.template get<"rigid_body">();
+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 = body.template get<"position">();
+	auto& pos_old = body.template get<"position_old">();
 
-		auto& rot = body.template get<"rotation">();
-		auto& acc = body.template get<"acceleration">();
+	auto& rot = body.template get<"rotation">();
+	auto& acc = body.template get<"acceleration">();
 
-		auto tsd_squared = time_step_delta * time_step_delta;
+	auto tsd_squared = time_step_delta * time_step_delta;
 
-		saw::data<sch::Vector<T,D>> pos_new;
-		// Actual step
-		for(uint64_t i = 0u; i < D; ++i){
-			pos_new.at({{i}}) = saw::data<T>{2.0} * pos.at({{i}}) - pos_old.at({{i}}) + acc.at({{i}}) * tsd_squared;
-		}
+	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 + acc * tsd_squared;
 
-		pos_old = pos;
-		pos = pos_new;
+	pos_old = pos;
+	pos = pos_new;
 };
 
 template<typename T, uint64_t D>
-constexpr auto handle_collision = [](saw::data<sch::Particle<T,D>>& left, saw::data<sch::Particle<T,D>>& right, saw::data<sch::Vector<T,D>> unit_pos_rel, saw::data<sch::Vector<T,D>> vel_rel)
+constexpr auto handle_collision = [](saw::data<sch::Particle<T,D>>& left, saw::data<sch::Particle<T,D>>& right, 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;
 	auto& mass_l = left.template get<"mass">();
+	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;
 	auto& mass_r = left.template get<"mass">();
+	auto vel_r = (pos_r-pos_old_r)/d_t;
 
-	/**
-	 * E to 0
-	 */
-
-	constexpr saw::data<sch::Scalar<T>> elasticity{0u};
-	constexpr saw::data<sch::Scalar<T>> one{1.0};
-
-	saw::data<sch::Scalar<T>> j = (e - one) / ((one / mass_l) + (one / mass_r));
-
-	vel_l = vel_l + vel_rel * j;
-	vel_r = vel_r - vel_rel * j;
+	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;
+	}
 };
 
 
diff --git a/lib/core/tests/particles.cpp b/lib/core/tests/particles.cpp
index 366c2a2..b2581f7 100644
--- a/lib/core/tests/particles.cpp
+++ b/lib/core/tests/particles.cpp
@@ -22,7 +22,9 @@ SAW_TEST("Verlet step 2D - Planar"){
 
 	acc.at({{0}}).set({1.0});
 
-	lbm::verlet_step_lambda<sch::T,2u>(particle,{0.5});
+	saw::data<sch::Scalar<sch::T>> dt;
+	dt.at({}).set(0.5);
+	lbm::verlet_step_lambda<sch::T,2u>(particle,dt);
 
 	SAW_EXPECT(pos.at({{0}}).get() == 0.25, std::string{"Incorrect Pos X: "} + std::to_string(pos.at({{0}}).get()));
 	SAW_EXPECT(pos.at({{1}}).get() == 0.0, std::string{"Incorrect Pos Y: "} + std::to_string(pos.at({{1}}).get()));
@@ -130,11 +132,13 @@ SAW_TEST("Moving particles 2D"){
 		pos_old = pos;
 		acc.at({{0u}}) = -0.1;
 	}
-	
+
+	saw::data<sch::Scalar<sch::T>> dt;
+	dt.at({}).set(0.5);
 	bool has_collided = false;
 	for(uint64_t i = 0u; i < 32u; ++i){
-		lbm::verlet_step_lambda<sch::T,2u>(part_a,{0.5});
-		lbm::verlet_step_lambda<sch::T,2u>(part_b,{0.5});
+		lbm::verlet_step_lambda<sch::T,2u>(part_a,dt);
+		lbm::verlet_step_lambda<sch::T,2u>(part_b,dt);
 
 		has_collided = lbm::broadphase_collision_check<sch::T,2u>(part_a,part_b);
 		if(has_collided){
-- 
cgit v1.2.3