From 2c77a360837974f2730bfbe8aad746bb6786d3db Mon Sep 17 00:00:00 2001
From: Quaternions <krakow20@gmail.com>
Date: Thu, 22 Aug 2024 13:41:41 -0700
Subject: [PATCH] tentative push solve implementation

---
 src/physics.rs | 28 ++++++++++++++++++----------
 1 file changed, 18 insertions(+), 10 deletions(-)

diff --git a/src/physics.rs b/src/physics.rs
index 4a78d37..fcaf958 100644
--- a/src/physics.rs
+++ b/src/physics.rs
@@ -763,23 +763,31 @@ impl TouchingState{
 		a
 	}
 	fn constrain_velocity(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,velocity:&mut Planar64Vec3){
-		//TODO: trey push solve
-		for contact in &self.contacts{
+		let contacts=self.contacts.iter().map(|contact|{
 			let n=contact_normal(models,hitbox_mesh,contact);
-			let d=n.dot128(*velocity);
-			if d<0{
-				*velocity-=n*Planar64::raw(((d<<32)/n.dot128(n)) as i64);
+			crate::push_solve::Contact{
+				position:Planar64Vec3::ZERO,
+				velocity:n,
+				normal:n,
 			}
+		}).collect();
+		match crate::push_solve::push_solve(&contacts,*velocity){
+			Some(new_velocity)=>*velocity=new_velocity,
+			None=>println!("Algorithm silently failing :)"),
 		}
 	}
 	fn constrain_acceleration(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,acceleration:&mut Planar64Vec3){
-		//TODO: trey push solve
-		for contact in &self.contacts{
+		let contacts=self.contacts.iter().map(|contact|{
 			let n=contact_normal(models,hitbox_mesh,contact);
-			let d=n.dot128(*acceleration);
-			if d<0{
-				*acceleration-=n*Planar64::raw(((d<<32)/n.dot128(n)) as i64);
+			crate::push_solve::Contact{
+				position:Planar64Vec3::ZERO,
+				velocity:n,
+				normal:n,
 			}
+		}).collect();
+		match crate::push_solve::push_solve(&contacts,*acceleration){
+			Some(new_acceleration)=>*acceleration=new_acceleration,
+			None=>println!("Algorithm silently failing :)"),
 		}
 	}
 	fn predict_collision_end(&self,collector:&mut instruction::InstructionCollector<PhysicsInternalInstruction>,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,body:&Body,time:Time){