more wip

Cargo.lock

@@ -105,9 +105,9 @@ checksum = "9d151e35f61089500b617991b791fc8bfd237ae50cd5950803758a179b41e67a"
 
 [[package]]
 name = "arrayvec"
-version = "0.7.4"
+version = "0.7.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "96d30a06541fbafbc7f82ed10c06164cfbd2c401138f6addd8404629c4b16711"
+checksum = "7c02d123df017efcdfbd739ef81735b36c5ba83ec3c59c80a9d7ecc718f92e50"
 
 [[package]]
 name = "as-raw-xcb-connection"
@@ -1882,6 +1882,7 @@ checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f"
 name = "strafe-client"
 version = "0.10.3"
 dependencies = [
+ "arrayvec",
  "bytemuck",
  "configparser",
  "ddsfile",

Cargo.toml

@@ -15,6 +15,7 @@ source = ["dep:strafesnet_deferred_loader", "dep:strafesnet_bsp_loader"]
 roblox = ["dep:strafesnet_deferred_loader", "dep:strafesnet_rbx_loader"]
 
 [dependencies]
+arrayvec = "0.7.6"
 bytemuck = { version = "1.13.1", features = ["derive"] }
 configparser = "3.0.2"
 ddsfile = "0.5.1"

src/main.rs

@@ -5,6 +5,7 @@ mod worker;
 mod physics;
 mod graphics;
 mod settings;
+mod push_solve;
 mod face_crawler;
 mod compat_worker;
 mod model_physics;

src/model_physics.rs

@@ -342,9 +342,9 @@ impl TryFrom<&model::Mesh> for PhysicsMesh{
 						let v0=mesh.unique_pos[vert0_id.get() as usize];
 						let v1=mesh.unique_pos[vert1_id.get() as usize];
 						normal+=Planar64Vec3::new(
-							(v0.y()-v1.y())*(v0.z()+v1.z()),
-							(v0.z()-v1.z())*(v0.x()+v1.x()),
-							(v0.x()-v1.x())*(v0.y()+v1.y()),
+							(v0.y-v1.y)*(v0.z+v1.z),
+							(v0.z-v1.z)*(v0.x+v1.x),
+							(v0.x-v1.x)*(v0.y+v1.y),
 						);
 						//get/create edge and push face into it
 						let (edge_ref_verts,is_sorted)=EdgeRefVerts::new(submesh_vert0_id,submesh_vert1_id);
@@ -955,7 +955,7 @@ fn is_empty_volume(normals:Vec<Planar64Vec3>)->bool{
 				if k!=i&&k!=j{
 					let d=n.dot(normals[k]);
 					if let Some(comp)=&d_comp{
-						if *comp*d<Planar64::ZERO{
+						if comp.is_negative()&&d.is_negative()||comp.is_positive()&&d.is_positive(){
 							return true;
 						}
 					}else{

src/physics.rs

@@ -11,7 +11,7 @@ use strafesnet_common::gameplay_modes::{self,StageId};
 use strafesnet_common::gameplay_style::{self,StyleModifiers};
 use strafesnet_common::controls_bitflag::Controls;
 use strafesnet_common::instruction::{self,InstructionEmitter,InstructionConsumer,TimedInstruction};
-use strafesnet_common::integer::{self,Time,Planar64,Planar64Vec3,Planar64Mat3,Angle32,Ratio64Vec2};
+use strafesnet_common::integer::{self,vec3,mat3,Time,Planar64,Planar64Vec3,Planar64Mat3,Angle32,Ratio64Vec2};
 use gameplay::ModeState;
 
 //external influence
@@ -36,7 +36,7 @@ enum PhysicsInstruction{
 	Input(PhysicsInputInstruction),
 }
 
-#[derive(Clone,Copy,Debug,Default,Hash)]
+#[derive(Clone,Copy,Debug,Hash)]
 pub struct Body{
 	pub position:Planar64Vec3,//I64 where 2^32 = 1 u
 	pub velocity:Planar64Vec3,//I64 where 2^32 = 1 u/s
@@ -124,13 +124,13 @@ struct ContactMoveState{
 }
 impl TransientAcceleration{
 	fn with_target_diff(target_diff:Planar64Vec3,accel:Planar64,time:Time)->Self{
-		if target_diff==Planar64Vec3::ZERO{
+		if target_diff==vec3::ZERO{
 			TransientAcceleration::Reached
 		}else{
 			//normal friction acceleration is clippedAcceleration.dot(normal)*friction
 			TransientAcceleration::Reachable{
-				acceleration:target_diff.with_length(accel),
-				time:time+Time::from(target_diff.length()/accel)
+				acceleration:target_diff.with_length(accel).divide().fix_1(),
+				time:time+Time::from((target_diff.length()/accel).divide().fix_1())
 			}
 		}
 	}
@@ -147,7 +147,7 @@ impl TransientAcceleration{
 	}
 	fn acceleration(&self)->Planar64Vec3{
 		match self{
-			TransientAcceleration::Reached=>Planar64Vec3::ZERO,
+			TransientAcceleration::Reached=>vec3::ZERO,
 			&TransientAcceleration::Reachable{acceleration,time:_}=>acceleration,
 			&TransientAcceleration::Unreachable{acceleration}=>acceleration,
 		}
@@ -158,7 +158,7 @@ impl ContactMoveState{
 		Self{
 			target:TransientAcceleration::ground(walk_settings,body,gravity,target_velocity),
 			contact,
-			jump_direction:JumpDirection::Exactly(Planar64Vec3::Y),
+			jump_direction:JumpDirection::Exactly(vec3::Y),
 		}
 	}
 	fn ladder(ladder_settings:&gameplay_style::LadderSettings,body:&Body,gravity:Planar64Vec3,target_velocity:Planar64Vec3,contact:ContactCollision)->Self{
@@ -296,7 +296,7 @@ impl PhysicsCamera{
 		let ay=Angle32::clamp_from_i64(a.y)
 		//clamp to actual vertical cam limit
 		.clamp(Self::ANGLE_PITCH_LOWER_LIMIT,Self::ANGLE_PITCH_UPPER_LIMIT);
-		Planar64Mat3::from_rotation_yx(ax,ay)
+		mat3::from_rotation_yx(ax,ay)
 	}
 	fn rotation(&self)->Planar64Mat3{
 		self.get_rotation(self.clamped_mouse_pos)
@@ -306,7 +306,7 @@ impl PhysicsCamera{
 	}
 	fn get_rotation_y(&self,mouse_pos_x:i32)->Planar64Mat3{
 		let ax=-self.sensitivity.x.mul_int(mouse_pos_x as i64);
-		Planar64Mat3::from_rotation_y(Angle32::wrap_from_i64(ax))
+		mat3::from_rotation_y(Angle32::wrap_from_i64(ax))
 	}
 	fn rotation_y(&self)->Planar64Mat3{
 		self.get_rotation_y(self.clamped_mouse_pos.x)
@@ -413,7 +413,7 @@ impl HitboxMesh{
 			aabb.grow(vert);
 		}
 		Self{
-			halfsize:aabb.size()/2,
+			halfsize:aabb.size()>>1,
 			mesh,
 			transform,
 		}
@@ -438,7 +438,7 @@ impl StyleHelper for StyleModifiers{
 
 	fn get_control_dir(&self,controls:Controls)->Planar64Vec3{
 		//don't get fancy just do it
-		let mut control_dir:Planar64Vec3 = Planar64Vec3::ZERO;
+		let mut control_dir:Planar64Vec3 = vec3::ZERO;
 		//Apply mask after held check so you can require non-allowed keys to be held for some reason
 		let controls=controls.intersection(self.controls_mask);
 		if controls.contains(Controls::MoveForward){
@@ -463,19 +463,19 @@ impl StyleHelper for StyleModifiers{
 	}
 
 	fn get_y_control_dir(&self,camera:&PhysicsCamera,controls:Controls)->Planar64Vec3{
-		camera.rotation_y()*self.get_control_dir(controls)
+		(camera.rotation_y()*self.get_control_dir(controls)).fix_1()
 	}
 
 	fn get_propulsion_control_dir(&self,camera:&PhysicsCamera,controls:Controls)->Planar64Vec3{
 		//don't interpolate this! discrete mouse movement, constant acceleration
-		camera.rotation()*self.get_control_dir(controls)
+		(camera.rotation()*self.get_control_dir(controls)).fix_1()
 	}
 	fn calculate_mesh(&self)->HitboxMesh{
 		let mesh=match self.hitbox.mesh{
 			gameplay_style::HitboxMesh::Box=>PhysicsMesh::unit_cube(),
 			gameplay_style::HitboxMesh::Cylinder=>PhysicsMesh::unit_cylinder(),
 		};
-		let transform=integer::Planar64Affine3::new(Planar64Mat3::from_diagonal(self.hitbox.halfsize),Planar64Vec3::ZERO);
+		let transform=mat3::from_diagonal(self.hitbox.halfsize).extend_column(vec3::ZERO);
 		HitboxMesh::new(mesh,transform)
 	}
 }
@@ -491,7 +491,7 @@ impl MoveState{
 	//call this after state.move_state is changed
 	fn apply_enum(&self,body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
 		match self{
-			MoveState::Fly=>body.acceleration=Planar64Vec3::ZERO,
+			MoveState::Fly=>body.acceleration=vec3::ZERO,
 			MoveState::Air=>{
 				//calculate base acceleration
 				let a=touching.base_acceleration(models,style,camera,input_state);
@@ -763,27 +763,35 @@ 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:vec3::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:vec3::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){
-		let relative_body=VirtualBody::relative(&Body::default(),body).body(time);
+		let relative_body=VirtualBody::relative(&Body::ZERO,body).body(time);
 		for contact in &self.contacts{
 			//detect face slide off
 			let model_mesh=models.contact_mesh(contact);
@@ -814,6 +822,7 @@ impl TouchingState{
 }
 
 impl Body{
+	pub const ZERO:Self=Self::new(vec3::ZERO,vec3::ZERO,vec3::ZERO,Time::ZERO);
 	pub const fn new(position:Planar64Vec3,velocity:Planar64Vec3,acceleration:Planar64Vec3,time:Time)->Self{
 		Self{
 			position,
@@ -824,11 +833,13 @@ impl Body{
 	}
 	pub fn extrapolated_position(&self,time:Time)->Planar64Vec3{
 		let dt=time-self.time;
-		self.position+self.velocity*dt+self.acceleration*(dt*dt/2)
+		self.position
+		+(self.velocity*dt).map(|elem|elem.divide().fix_1())
+		+self.acceleration.map(|elem|(dt*dt*elem/2).divide().fix_1())
 	}
 	pub fn extrapolated_velocity(&self,time:Time)->Planar64Vec3{
 		let dt=time-self.time;
-		self.velocity+self.acceleration*dt
+		self.velocity+(self.acceleration*dt).map(|elem|elem.divide().fix_1())
 	}
 	pub fn advance_time(&mut self,time:Time){
 		self.position=self.extrapolated_position(time);
@@ -836,8 +847,8 @@ impl Body{
 		self.time=time;
 	}
 	pub fn infinity_dir(&self)->Option<Planar64Vec3>{
-		if self.velocity==Planar64Vec3::ZERO{
-			if self.acceleration==Planar64Vec3::ZERO{
+		if self.velocity==vec3::ZERO{
+			if self.acceleration==vec3::ZERO{
 				None
 			}else{
 				Some(self.acceleration)
@@ -851,20 +862,20 @@ impl Body{
 		aabb.grow(self.extrapolated_position(t1));
 		//v+a*t==0
 		//goober code
-		if self.acceleration.x()!=Planar64::ZERO{
-			let t=Time::from(-self.velocity.x()/self.acceleration.x());
+		if !self.acceleration.x.is_zero(){
+			let t=Time::from(-self.velocity.x/self.acceleration.x);
 			if t0<t&&t<t1{
 				aabb.grow(self.extrapolated_position(t));
 			}
 		}
-		if self.acceleration.y()!=Planar64::ZERO{
-			let t=Time::from(-self.velocity.y()/self.acceleration.y());
+		if !self.acceleration.y.is_zero(){
+			let t=Time::from(-self.velocity.y/self.acceleration.y);
 			if t0<t&&t<t1{
 				aabb.grow(self.extrapolated_position(t));
 			}
 		}
-		if self.acceleration.z()!=Planar64::ZERO{
-			let t=Time::from(-self.velocity.z()/self.acceleration.z());
+		if !self.acceleration.z.is_zero(){
+			let t=Time::from(-self.velocity.z/self.acceleration.z);
 			if t0<t&&t<t1{
 				aabb.grow(self.extrapolated_position(t));
 			}
@@ -939,7 +950,7 @@ pub struct PhysicsData{
 impl Default for PhysicsState{
 	fn default()->Self{
  		Self{
-			body:Body::new(Planar64Vec3::int(0,50,0),Planar64Vec3::int(0,0,0),Planar64Vec3::int(0,-100,0),Time::ZERO),
+			body:Body::new(vec3::int(0,50,0),vec3::int(0,0,0),vec3::int(0,-100,0),Time::ZERO),
 			time:Time::ZERO,
 			style:StyleModifiers::default(),
 			touching:TouchingState::default(),
@@ -1331,12 +1342,12 @@ fn set_velocity_cull(body:&mut Body,touching:&mut TouchingState,models:&PhysicsM
 	let mut culled=false;
 	touching.contacts.retain(|contact|{
 		let n=contact_normal(models,hitbox_mesh,contact);
-		let r=n.dot(v)<=Planar64::ZERO;
-		if !r{
+		let r=n.dot(v).is_positive();
+		if r{
 			culled=true;
 			println!("set_velocity_cull contact={:?}",contact);
 		}
-		r
+		!r
 	});
 	set_velocity(body,touching,models,hitbox_mesh,v);
 	culled
@@ -1350,12 +1361,12 @@ fn set_acceleration_cull(body:&mut Body,touching:&mut TouchingState,models:&Phys
 	let mut culled=false;
 	touching.contacts.retain(|contact|{
 		let n=contact_normal(models,hitbox_mesh,contact);
-		let r=n.dot(a)<=Planar64::ZERO;
-		if !r{
+		let r=n.dot(a).is_positive();
+		if r{
 			culled=true;
 			println!("set_acceleration_cull contact={:?}",contact);
 		}
-		r
+		!r
 	});
 	set_acceleration(body,touching,models,hitbox_mesh,a);
 	culled
@@ -1403,8 +1414,9 @@ fn teleport_to_spawn(
 	input_state:&InputState,
 	time:Time,
 )->Result<(),TeleportToSpawnError>{
+	const EPSILON:Planar64=Planar64::raw((1<<32)/16);
 	let transform=models.get_model_transform(stage.spawn()).ok_or(TeleportToSpawnError::NoModel)?;
-	let point=transform.vertex.transform_point3(Planar64Vec3::Y)+Planar64Vec3::Y*(style.hitbox.halfsize.y()+Planar64::ONE/16);
+	let point=transform.vertex.y_axis+Planar64Vec3::new([Planar64::ZERO,style.hitbox.halfsize.y+EPSILON,Planar64::ZERO]);
 	teleport(point,move_state,body,touching,run,mode_state,Some(mode),models,hitbox_mesh,bvh,style,camera,input_state,time);
 	Ok(())
 }
@@ -1492,7 +1504,7 @@ fn run_teleport_behaviour(
 	}
 	if let Some(&gameplay_attributes::Wormhole{destination_model})=wormhole{
 		if let (Some(origin),Some(destination))=(models.get_model_transform(model_id),models.get_model_transform(destination_model)){
-			let point=body.position-origin.vertex.translation+destination.vertex.translation;
+			let point=body.position-origin.vertex.col(3)+destination.vertex.col(3);
 			//TODO: camera angles
 			teleport(point,move_state,body,touching,run,mode_state,mode,models,hitbox_mesh,bvh,style,camera,input_state,time);
 		}
@@ -1534,7 +1546,7 @@ fn collision_start_contact(
 				//kill v
 				//actually you could do this with a booster attribute :thinking:
 				//it's a little bit different because maybe you want to chain ladders together
-				set_velocity(body,touching,models,hitbox_mesh,Planar64Vec3::ZERO);//model.velocity
+				set_velocity(body,touching,models,hitbox_mesh,vec3::ZERO);//model.velocity
 			}
 			//ladder walkstate
 			let (gravity,target_velocity)=ladder_things(ladder_settings,&contact,touching,models,hitbox_mesh,style,camera,input_state);
@@ -1724,9 +1736,9 @@ fn atomic_internal_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:Tim
 					if strafe_settings.activates(controls){
 						let masked_controls=strafe_settings.mask(controls);
 						let control_dir=state.style.get_control_dir(masked_controls);
-						if control_dir!=Planar64Vec3::ZERO{
+						if control_dir!=vec3::ZERO{
 							let camera_mat=state.camera.simulate_move_rotation_y(state.input_state.lerp_delta(state.time).x);
-							if let Some(ticked_velocity)=strafe_settings.tick_velocity(state.body.velocity,(camera_mat*control_dir).with_length(Planar64::ONE)){
+							if let Some(ticked_velocity)=strafe_settings.tick_velocity(state.body.velocity,(camera_mat*control_dir).with_length(Planar64::ONE).divide().fix_1()){
 								//this is wrong but will work ig
 								//need to note which push planes activate in push solve and keep those
 								state.cull_velocity(data,ticked_velocity);
@@ -1750,7 +1762,7 @@ fn atomic_internal_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:Tim
 								//we know that the acceleration is precisely zero because the walk target is known to be reachable
 								//which means that gravity can be fully cancelled
 								//ignore moving platforms for now
-								set_acceleration(&mut state.body,&state.touching,&data.models,&data.hitbox_mesh,Planar64Vec3::ZERO);
+								set_acceleration(&mut state.body,&state.touching,&data.models,&data.hitbox_mesh,vec3::ZERO);
 								walk_state.target=TransientAcceleration::Reached;
 							},
 							//you are not supposed to reach an unreachable walk target!
@@ -1843,11 +1855,11 @@ fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedI
 							//TODO: spawn at the bottom of the start zone plus the hitbox size
 							//TODO: set camera andles to face the same way as the start zone
 							data.models.get_model_transform(mode.get_start().into()).map(|transform|
-								transform.vertex.translation
+								transform.vertex.col(3)
 							)
-						).unwrap_or(Planar64Vec3::ZERO);
+						).unwrap_or(vec3::ZERO);
 						set_position(spawn_point,&mut state.move_state,&mut state.body,&mut state.touching,&mut state.run,&mut state.mode_state,mode,&data.models,&data.hitbox_mesh,&data.bvh,&state.style,&state.camera,&state.input_state,state.time);
-						set_velocity(&mut state.body,&state.touching,&data.models,&data.hitbox_mesh,Planar64Vec3::ZERO);
+						set_velocity(&mut state.body,&state.touching,&data.models,&data.hitbox_mesh,vec3::ZERO);
 						state.set_move_state(data,MoveState::Air);
 						b_refresh_walk_target=false;
 					}
@@ -1910,9 +1922,10 @@ fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedI
 
 #[cfg(test)]
 mod test{
+	use strafesnet_common::integer::{vec3::{self,int as int3},mat3};
 	use super::*;
 	fn test_collision_axis_aligned(relative_body:Body,expected_collision_time:Option<Time>){
-		let h0=HitboxMesh::new(PhysicsMesh::unit_cube(),integer::Planar64Affine3::new(Planar64Mat3::from_diagonal(Planar64Vec3::int(5,1,5)/2),Planar64Vec3::ZERO));
+		let h0=HitboxMesh::new(PhysicsMesh::unit_cube(),mat3::from_diagonal(int3(5,1,5)>>1).extend_column(vec3::ZERO));
 		let h1=StyleModifiers::roblox_bhop().calculate_mesh();
 		let hitbox_mesh=h1.transformed_mesh();
 		let platform_mesh=h0.transformed_mesh();
@@ -1922,14 +1935,11 @@ mod test{
 	}
 	fn test_collision_rotated(relative_body:Body,expected_collision_time:Option<Time>){
 		let h0=HitboxMesh::new(PhysicsMesh::unit_cube(),
-			integer::Planar64Affine3::new(
-				integer::Planar64Mat3::from_cols(
-					Planar64Vec3::int(5,0,1)/2,
-					Planar64Vec3::int(0,1,0)/2,
-					Planar64Vec3::int(-1,0,5)/2,
-				),
-				Planar64Vec3::ZERO,
-			)
+			integer::Planar64Mat3::from_rows([
+				int3(5,0,1)>>1,
+				int3(0,1,0)>>1,
+				int3(-1,0,5)>>1,
+			]).extend_column(vec3::ZERO),
 		);
 		let h1=StyleModifiers::roblox_bhop().calculate_mesh();
 		let hitbox_mesh=h1.transformed_mesh();
@@ -1945,180 +1955,180 @@ mod test{
 	#[test]
 	fn test_collision_degenerate(){
 		test_collision(Body::new(
-			Planar64Vec3::int(0,5,0),
-			Planar64Vec3::int(0,-1,0),
-			Planar64Vec3::ZERO,
+			int3(0,5,0),
+			int3(0,-1,0),
+			vec3::ZERO,
 			Time::ZERO
 		),Some(Time::from_secs(2)));
 	}
 	#[test]
 	fn test_collision_degenerate_east(){
 		test_collision(Body::new(
-			Planar64Vec3::int(3,5,0),
-			Planar64Vec3::int(0,-1,0),
-			Planar64Vec3::ZERO,
+			int3(3,5,0),
+			int3(0,-1,0),
+			vec3::ZERO,
 			Time::ZERO
 		),Some(Time::from_secs(2)));
 	}
 	#[test]
 	fn test_collision_degenerate_south(){
 		test_collision(Body::new(
-			Planar64Vec3::int(0,5,3),
-			Planar64Vec3::int(0,-1,0),
-			Planar64Vec3::ZERO,
+			int3(0,5,3),
+			int3(0,-1,0),
+			vec3::ZERO,
 			Time::ZERO
 		),Some(Time::from_secs(2)));
 	}
 	#[test]
 	fn test_collision_degenerate_west(){
 		test_collision(Body::new(
-			Planar64Vec3::int(-3,5,0),
-			Planar64Vec3::int(0,-1,0),
-			Planar64Vec3::ZERO,
+			int3(-3,5,0),
+			int3(0,-1,0),
+			vec3::ZERO,
 			Time::ZERO
 		),Some(Time::from_secs(2)));
 	}
 	#[test]
 	fn test_collision_degenerate_north(){
 		test_collision(Body::new(
-			Planar64Vec3::int(0,5,-3),
-			Planar64Vec3::int(0,-1,0),
-			Planar64Vec3::ZERO,
+			int3(0,5,-3),
+			int3(0,-1,0),
+			vec3::ZERO,
 			Time::ZERO
 		),Some(Time::from_secs(2)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_east_from_west(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(3,3,0),
-			Planar64Vec3::int(100,-1,0),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(3,3,0),
+			int3(100,-1,0),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_south_from_north(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(0,3,3),
-			Planar64Vec3::int(0,-1,100),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(0,3,3),
+			int3(0,-1,100),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_west_from_east(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(-3,3,0),
-			Planar64Vec3::int(-100,-1,0),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(-3,3,0),
+			int3(-100,-1,0),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_north_from_south(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(0,3,-3),
-			Planar64Vec3::int(0,-1,-100),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(0,3,-3),
+			int3(0,-1,-100),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_north_from_ne(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(0,6,-7)/2,
-			Planar64Vec3::int(-10,-1,1),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(0,6,-7)>>1,
+			int3(-10,-1,1),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_north_from_nw(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(0,6,-7)/2,
-			Planar64Vec3::int(10,-1,1),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(0,6,-7)>>1,
+			int3(10,-1,1),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_east_from_se(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(7,6,0)/2,
-			Planar64Vec3::int(-1,-1,-10),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(7,6,0)>>1,
+			int3(-1,-1,-10),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_east_from_ne(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(7,6,0)/2,
-			Planar64Vec3::int(-1,-1,10),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(7,6,0)>>1,
+			int3(-1,-1,10),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_south_from_se(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(0,6,7)/2,
-			Planar64Vec3::int(-10,-1,-1),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(0,6,7)>>1,
+			int3(-10,-1,-1),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_south_from_sw(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(0,6,7)/2,
-			Planar64Vec3::int(10,-1,-1),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(0,6,7)>>1,
+			int3(10,-1,-1),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_west_from_se(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(-7,6,0)/2,
-			Planar64Vec3::int(1,-1,-10),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(-7,6,0)>>1,
+			int3(1,-1,-10),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_parabola_edge_west_from_ne(){
-		test_collision(VirtualBody::relative(&Body::default(),&Body::new(
-			Planar64Vec3::int(-7,6,0)/2,
-			Planar64Vec3::int(1,-1,10),
-			Planar64Vec3::int(0,-1,0),
+		test_collision(VirtualBody::relative(&Body::ZERO,&Body::new(
+			int3(-7,6,0)>>1,
+			int3(1,-1,10),
+			int3(0,-1,0),
 			Time::ZERO
 		)).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
 	}
 	#[test]
 	fn test_collision_oblique(){
 		test_collision(Body::new(
-			Planar64Vec3::int(0,5,0),
-			Planar64Vec3::int(1,-64,2)/64,
-			Planar64Vec3::ZERO,
+			int3(0,5,0),
+			int3(1,-64,2)>>6,// /64
+			vec3::ZERO,
 			Time::ZERO
 		),Some(Time::from_secs(2)));
 	}
 	#[test]
 	fn zoom_hit_nothing(){
 		test_collision(Body::new(
-			Planar64Vec3::int(0,10,0),
-			Planar64Vec3::int(1,0,0),
-			Planar64Vec3::int(0,1,0),
+			int3(0,10,0),
+			int3(1,0,0),
+			int3(0,1,0),
 			Time::ZERO
 		),None);
 	}
 	#[test]
 	fn already_inside_hit_nothing(){
 		test_collision(Body::new(
-			Planar64Vec3::ZERO,
-			Planar64Vec3::int(1,0,0),
-			Planar64Vec3::int(0,1,0),
+			vec3::ZERO,
+			int3(1,0,0),
+			int3(0,1,0),
 			Time::ZERO
 		),None);
 	}

src/push_solve.rs

@@ -0,0 +1,339 @@
+use strafesnet_common::integer::{Planar64,Planar64Vec3};
+
+// This algorithm is based on Lua code
+// written by Trey Reynolds in 2021
+
+// 1/2^10
+const EPSILON:Planar64=Planar64::raw(1<<(32-10));
+
+// A stack-allocated variable-size list that holds up to 4 elements
+// Direct references are used instead of indices i0, i1, i2, i3
+type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>;
+
+struct Ray{
+	origin:Planar64Vec3,
+	direction:Planar64Vec3,
+}
+impl Ray{
+	fn extrapolate(&self,t:Planar64)->Planar64Vec3{
+		self.origin+self.direction*t
+	}
+}
+
+/// Information about a contact restriction
+pub struct Contact{
+	pub position:Planar64Vec3,
+	pub velocity:Planar64Vec3,
+	pub normal:Planar64Vec3,
+}
+impl Contact{
+	fn relative_to(&self,point:Planar64Vec3)->Self{
+		Self{
+			position:self.position-point,
+			velocity:self.velocity,
+			normal:self.normal,
+		}
+	}
+	fn relative_dot(&self,direction:Planar64Vec3)->Planar64{
+		(direction-self.velocity).dot(self.normal)
+	}
+	/// Calculate the time of intersection. (previously get_touch_time)
+	fn solve(&self,ray:&Ray)->Planar64{
+		(self.position-ray.origin).dot(self.normal)/(ray.direction-self.velocity).dot(self.normal)
+	}
+}
+
+//note that this is horrible with fixed point arithmetic
+fn solve1(c0:&Contact)->Option<Planar64Vec3>{
+	let det=c0.normal.dot(c0.velocity);
+	if det.get().abs()<EPSILON.get(){
+		return None;
+	}
+	let d0=c0.normal.dot(c0.position);
+	Some(c0.normal*d0/det)
+}
+fn solve2(c0:&Contact,c1:&Contact)->Option<Planar64Vec3>{
+	let u0_u1=c0.velocity.cross(c1.velocity);
+	let n0_n1=c0.normal.cross(c1.normal);
+	let det=u0_u1.dot(n0_n1);
+	if det.get().abs()<EPSILON.get(){
+		return None;
+	}
+	let d0=c0.normal.dot(c0.position);
+	let d1=c1.normal.dot(c1.position);
+	Some((c1.normal.cross(u0_u1)*d0+u0_u1.cross(c0.normal)*d1)/det)
+}
+fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Option<Planar64Vec3>{
+	let n0_n1=c0.normal.cross(c1.normal);
+	let det=c2.normal.dot(n0_n1);
+	if det.get().abs()<EPSILON.get(){
+		return None;
+	}
+	let d0=c0.normal.dot(c0.position);
+	let d1=c1.normal.dot(c1.position);
+	let d2=c2.normal.dot(c2.position);
+	Some((c1.normal.cross(c2.normal)*d0+c2.normal.cross(c0.normal)*d1+c0.normal.cross(c1.normal)*d2)/det)
+}
+
+fn decompose1(point:Planar64Vec3,u0:Planar64Vec3)->Option<Planar64>{
+	let det=u0.dot(u0);
+	if det==Planar64::ZERO{
+		return None;
+	}
+	let s0=u0.dot(point)/det;
+	Some(s0)
+}
+fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->Option<(Planar64,Planar64)>{
+	let u0_u1=u0.cross(u1);
+	let det=u0_u1.dot(u0_u1);
+	if det==Planar64::ZERO{
+		return None;
+	}
+	let s0=u0_u1.dot(point.cross(u1))/det;
+	let s1=u0_u1.dot(u0.cross(point))/det;
+	Some((s0,s1))
+}
+fn decompose3(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3,u2:Planar64Vec3)->Option<(Planar64,Planar64,Planar64)>{
+	let det=u0.cross(u1).dot(u2);
+	if det==Planar64::ZERO{
+		return None;
+	}
+	let s0=point.cross(u1).dot(u2)/det;
+	let s1=u0.cross(point).dot(u2)/det;
+	let s2=u0.cross(u1).dot(point)/det;
+	Some((s0,s1,s2))
+}
+
+fn is_space_enclosed_2(
+	a:Planar64Vec3,
+	b:Planar64Vec3,
+)->bool{
+	a.cross(b)==Planar64Vec3::ZERO
+	&&a.dot(b)<Planar64::ZERO
+}
+fn is_space_enclosed_3(
+	a:Planar64Vec3,
+	b:Planar64Vec3,
+	c:Planar64Vec3
+)->bool{
+	a.cross(b).dot(c)==Planar64::ZERO
+	&&{
+		let det_abac=a.cross(b).dot(a.cross(c));
+		let det_abbc=a.cross(b).dot(b.cross(c));
+		let det_acbc=a.cross(c).dot(b.cross(c));
+		return det_abac*det_abbc<=Planar64::ZERO
+			&& det_abbc*det_acbc<=Planar64::ZERO
+			&&-det_acbc*det_abac<=Planar64::ZERO
+			||is_space_enclosed_2(a,b)
+			||is_space_enclosed_2(a,c)
+			||is_space_enclosed_2(b,c)
+	}
+}
+fn is_space_enclosed_4(
+	a:Planar64Vec3,
+	b:Planar64Vec3,
+	c:Planar64Vec3,
+	d:Planar64Vec3,
+)->bool{
+	let det_abc=a.cross(b).dot(c);
+	let det_abd=a.cross(b).dot(d);
+	let det_acd=a.cross(c).dot(d);
+	let det_bcd=b.cross(c).dot(d);
+	return det_abc*det_abd<Planar64::ZERO
+		&&-det_abc*det_acd<Planar64::ZERO
+		&& det_abd*det_acd<Planar64::ZERO
+		&& det_abc*det_bcd<Planar64::ZERO
+		&&-det_abd*det_bcd<Planar64::ZERO
+		&& det_acd*det_bcd<Planar64::ZERO
+		||is_space_enclosed_3(a,b,c)
+		||is_space_enclosed_3(a,b,d)
+		||is_space_enclosed_3(a,c,d)
+		||is_space_enclosed_3(b,c,d)
+}
+
+const fn get_push_ray_0(point:Planar64Vec3)->Option<Ray>{
+	Some(Ray{origin:point,direction:Planar64Vec3::ZERO})
+}
+fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option<Ray>{
+	let direction=solve1(c0)?;
+	let s0=decompose1(direction,c0.velocity)?;
+	if s0<Planar64::ZERO{
+		return None;
+	}
+	let origin=point+solve1(
+		&c0.relative_to(point),
+	)?;
+	Some(Ray{origin,direction})
+}
+fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option<Ray>{
+	let direction=solve2(c0,c1)?;
+	let (s0,s1)=decompose2(direction,c0.velocity,c1.velocity)?;
+	if s0<Planar64::ZERO||s1<Planar64::ZERO{
+		return None;
+	}
+	let origin=point+solve2(
+		&c0.relative_to(point),
+		&c1.relative_to(point),
+	)?;
+	Some(Ray{origin,direction})
+}
+fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Option<Ray>{
+	let direction=solve3(c0,c1,c2)?;
+	let (s0,s1,s2)=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?;
+	if s0<Planar64::ZERO||s1<Planar64::ZERO||s2<Planar64::ZERO{
+		return None;
+	}
+	let origin=point+solve3(
+		&c0.relative_to(point),
+		&c1.relative_to(point),
+		&c2.relative_to(point),
+	)?;
+	Some(Ray{origin,direction})
+}
+
+const fn get_best_push_ray_and_conts_0<'a>(point:Planar64Vec3)->Option<(Ray,Conts<'a>)>{
+	match get_push_ray_0(point){
+		Some(ray)=>Some((ray,Conts::new_const())),
+		None=>None,
+	}
+}
+fn get_best_push_ray_and_conts_1(point:Planar64Vec3,c0:&Contact)->Option<(Ray,Conts)>{
+	get_push_ray_1(point,c0)
+		.map(|ray|(ray,Conts::from_iter([c0])))
+}
+fn get_best_push_ray_and_conts_2<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact)->Option<(Ray,Conts<'a>)>{
+	if is_space_enclosed_2(c0.normal,c1.normal){
+		return None;
+	}
+	if let Some(ray)=get_push_ray_2(point,c0,c1){
+		return Some((ray,Conts::from_iter([c0,c1])));
+	}
+	if let Some(ray)=get_push_ray_1(point,c0){
+		if Planar64::ZERO<=c1.relative_dot(ray.direction){
+			return Some((ray,Conts::from_iter([c0])));
+		}
+	}
+	return None;
+}
+fn get_best_push_ray_and_conts_3<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact)->Option<(Ray,Conts<'a>)>{
+	if is_space_enclosed_3(c0.normal,c1.normal,c2.normal){
+		return None;
+	}
+	if let Some(ray)=get_push_ray_3(point,c0,c1,c2){
+		return Some((ray,Conts::from_iter([c0,c1,c2])));
+	}
+	if let Some(ray)=get_push_ray_2(point,c0,c1){
+		if Planar64::ZERO<=c2.relative_dot(ray.direction){
+			return Some((ray,Conts::from_iter([c0,c1])));
+		}
+	}
+	if let Some(ray)=get_push_ray_2(point,c0,c2){
+		if Planar64::ZERO<=c1.relative_dot(ray.direction){
+			return Some((ray,Conts::from_iter([c0,c2])));
+		}
+	}
+	if let Some(ray)=get_push_ray_1(point,c0){
+		if Planar64::ZERO<=c1.relative_dot(ray.direction)
+		 &&Planar64::ZERO<=c2.relative_dot(ray.direction){
+			return Some((ray,Conts::from_iter([c0])));
+		}
+	}
+	return None;
+}
+fn get_best_push_ray_and_conts_4<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact,c3:&'a Contact)->Option<(Ray,Conts<'a>)>{
+	if is_space_enclosed_4(c0.normal,c1.normal,c2.normal,c3.normal){
+		return None;
+	}
+
+	let (ray012,conts012)=get_best_push_ray_and_conts_3(point,c0,c1,c2)?;
+	let (ray013,conts013)=get_best_push_ray_and_conts_3(point,c0,c1,c3)?;
+	let (ray023,conts023)=get_best_push_ray_and_conts_3(point,c0,c2,c3)?;
+
+	let err012=c3.relative_dot(ray012.direction);
+	let err013=c2.relative_dot(ray013.direction);
+	let err023=c1.relative_dot(ray023.direction);
+
+	let best_err=err012.max(err013).max(err023);
+
+	if best_err==err012{
+		return Some((ray012,conts012))
+	}else if best_err==err013{
+		return Some((ray013,conts013))
+	}else if best_err==err023{
+		return Some((ray023,conts023))
+	}
+	unreachable!()
+}
+
+fn get_best_push_ray_and_conts<'a>(
+	point:Planar64Vec3,
+	conts:Conts<'a>,
+)->Option<(Ray,Conts<'a>)>{
+	match conts.as_slice(){
+		&[c0,c1,c2,c3]=>get_best_push_ray_and_conts_4(point,c0,c1,c2,c3),
+		&[c0,c1,c2]=>get_best_push_ray_and_conts_3(point,c0,c1,c2),
+		&[c0,c1]=>get_best_push_ray_and_conts_2(point,c0,c1),
+		&[c0]=>get_best_push_ray_and_conts_1(point,c0),
+		&[]=>get_best_push_ray_and_conts_0(point),
+		_=>unreachable!(),
+	}
+}
+
+fn get_first_touch<'a>(contacts:&'a Vec<Contact>,ray:&Ray,conts:&Conts)->Option<(Planar64,&'a Contact)>{
+	contacts.iter()
+		.filter(|&contact|
+			!conts.iter().any(|&c|std::ptr::eq(c,contact))
+			&&contact.relative_dot(ray.direction)< -EPSILON
+		)
+		.map(|contact|(contact.solve(ray),contact))
+		.min_by_key(|&(t,_)|t)
+}
+
+pub fn push_solve(contacts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
+	let (mut ray,mut conts)=get_best_push_ray_and_conts_0(point)?;
+	loop{
+		let (next_t,next_cont)=match get_first_touch(contacts,&ray,&conts){
+			Some((t,conts))=>(t,conts),
+			None=>return Some(ray.origin),
+		};
+
+		if Planar64::ZERO<=next_t{
+			return Some(ray.origin);
+		}
+
+		//push_front
+		if conts.len()==conts.capacity(){
+			//this is a dead case, new_conts never has more than 3 elements
+			conts.rotate_right(1);
+			conts[0]=next_cont;
+		}else{
+			conts.push(next_cont);
+			conts.rotate_right(1);
+		}
+
+		let meet_point=ray.extrapolate(next_t);
+		match get_best_push_ray_and_conts(meet_point,conts){
+			Some((new_ray,new_conts))=>(ray,conts)=(new_ray,new_conts),
+			None=>return Some(meet_point),
+		}
+	}
+}
+
+#[cfg(test)]
+mod tests{
+	use super::*;
+	#[test]
+	fn test_push_solve(){
+		let contacts=vec![
+			Contact{
+				position:Planar64Vec3::ZERO,
+				velocity:Planar64Vec3::Y,
+				normal:Planar64Vec3::Y,
+			}
+		];
+		assert_eq!(
+			Some(Planar64Vec3::ZERO),
+			push_solve(&contacts,Planar64Vec3::NEG_Y)
+		);
+	}
+}