use push planes

engine/physics/src/physics.rs

@@ -174,7 +174,8 @@ fn ground_things(walk_settings:&gameplay_style::WalkSettings,contact:&ContactCol
 	let gravity=touching.base_acceleration(models,style,camera,input_state);
 	let control_dir=style.get_y_control_dir(camera,input_state.controls);
 	let target_velocity=walk_settings.get_walk_target_velocity(control_dir,normal);
-	let target_velocity_clipped=touching.constrain_velocity(models,hitbox_mesh,target_velocity);
+	let contacts=touching.contacts(models,hitbox_mesh);
+	let target_velocity_clipped=touching.constrain_velocity(&contacts,target_velocity).velocity;
 	(gravity,target_velocity_clipped)
 }
 fn ladder_things(ladder_settings:&gameplay_style::LadderSettings,contact:&ContactCollision,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState)->(Planar64Vec3,Planar64Vec3){
@@ -182,7 +183,8 @@ fn ladder_things(ladder_settings:&gameplay_style::LadderSettings,contact:&Contac
 	let gravity=touching.base_acceleration(models,style,camera,input_state);
 	let control_dir=style.get_y_control_dir(camera,input_state.controls);
 	let target_velocity=ladder_settings.get_ladder_target_velocity(control_dir,normal);
-	let target_velocity_clipped=touching.constrain_velocity(models,hitbox_mesh,target_velocity);
+	let contacts=touching.contacts(models,hitbox_mesh);
+	let target_velocity_clipped=touching.constrain_velocity(&contacts,target_velocity).velocity;
 	(gravity,target_velocity_clipped)
 }
 
@@ -520,7 +522,8 @@ impl MoveState{
 				// calculate base acceleration
 				let base_acceleration=touching.base_acceleration(models,style,camera,input_state);
 				// constrain_acceleration clips according to contacts
-				touching.constrain_acceleration(models,hitbox_mesh,base_acceleration)
+				let contacts=touching.contacts(models,hitbox_mesh);
+				touching.constrain_acceleration(&contacts,base_acceleration).acceleration
 			},
 			MoveState::Walk(walk_state)
 			|MoveState::Ladder(walk_state)
@@ -605,16 +608,18 @@ impl MoveState{
 		*self=move_state;
 		//this function call reads the above state that was just set
 		self.update_walk_target(body,touching,models,hitbox_mesh,style,camera,input_state);
+		// Never used? make body immutable
 		self.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state);
 	}
 	fn cull_velocity(&mut self,velocity:Planar64Vec3,body:&mut Body,touching:&mut TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
-		//TODO: be more precise about contacts
 		if set_velocity_cull(body,touching,models,hitbox_mesh,velocity){
-			// TODO do better
-			// TODO: unduplicate this code
+			self.touching_changed(body,touching,models,hitbox_mesh,style,camera,input_state,|contact|!touching.contains_contact(&contact.convex_mesh_id));
+		}
+	}
+	fn touching_changed(&mut self,body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState,is_contact_removed:impl Fn(&ContactCollision)->bool){
 		match self.get_walk_state(){
 			// did you stop touching the thing you were walking on?
-				Some(walk_state)=>if !touching.contains_contact(&walk_state.contact.convex_mesh_id){
+			Some(walk_state)=>if is_contact_removed(&walk_state.contact){
 				self.set_move_state(MoveState::Air,body,touching,models,hitbox_mesh,style,camera,input_state);
 			}else{
 				// stopped touching something else while walking
@@ -626,7 +631,6 @@ impl MoveState{
 		}
 	}
 }
-}
 
 #[derive(Clone,Hash,Eq,PartialEq)]
 enum PhysicsCollisionAttributes{
@@ -752,6 +756,18 @@ impl Collision{
 		}
 	}
 }
+struct Contacts<'a>{
+	contacts:Vec<crate::push_solve::Contact>,
+	lifetime:core::marker::PhantomData<&'a ()>,
+}
+struct ConstrainedVelocity<'a>{
+	velocity:Planar64Vec3,
+	constraints:crate::push_solve::Conts<'a>,
+}
+struct ConstrainedAcceleration<'a>{
+	acceleration:Planar64Vec3,
+	constraints:crate::push_solve::Conts<'a>,
+}
 #[derive(Clone,Debug,Default)]
 struct TouchingState{
 	// This is kind of jank, it's a ContactCollision
@@ -807,8 +823,8 @@ impl TouchingState{
 		//TODO: add water
 		a
 	}
-	fn constrain_velocity(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,velocity:Planar64Vec3)->Planar64Vec3{
-		let contacts:Vec<_>=self.contacts.iter().map(|(convex_mesh_id,face_id)|{
+	fn contacts<'a>(&'a self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh)->Contacts<'a>{
+		let contacts=self.contacts.iter().map(|(convex_mesh_id,face_id)|{
 			let n=contact_normal(models,hitbox_mesh,convex_mesh_id,*face_id);
 			crate::push_solve::Contact{
 				position:vec3::zero(),
@@ -816,18 +832,24 @@ impl TouchingState{
 				normal:n,
 			}
 		}).collect();
-		crate::push_solve::push_solve(&contacts,velocity).0
+		Contacts{
+			contacts,
+			lifetime:core::marker::PhantomData,
 		}
-	fn constrain_acceleration(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,acceleration:Planar64Vec3)->Planar64Vec3{
-		let contacts:Vec<_>=self.contacts.iter().map(|(convex_mesh_id,face_id)|{
-			let n=contact_normal(models,hitbox_mesh,convex_mesh_id,*face_id);
-			crate::push_solve::Contact{
-				position:vec3::zero(),
-				velocity:n,
-				normal:n,
 	}
-		}).collect();
-		crate::push_solve::push_solve(&contacts,acceleration).0
+	fn constrain_velocity<'a>(&self,contacts:&'a Contacts<'_>,velocity:Planar64Vec3)->ConstrainedVelocity<'a>{
+		let (velocity,constraints)=crate::push_solve::push_solve(&contacts.contacts,velocity);
+		ConstrainedVelocity{
+			velocity,
+			constraints
+		}
+	}
+	fn constrain_acceleration<'a>(&self,contacts:&'a Contacts<'_>,acceleration:Planar64Vec3)->ConstrainedAcceleration<'a>{
+		let (acceleration,constraints)=crate::push_solve::push_solve(&contacts.contacts,acceleration);
+		ConstrainedAcceleration{
+			acceleration,
+			constraints
+		}
 	}
 	fn predict_collision_end(&self,collector:&mut instruction::InstructionCollector<InternalInstruction,Time>,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,trajectory:&Trajectory,start_time:Time){
 		// let relative_body=body.relative_to(&Body::ZERO);
@@ -943,9 +965,6 @@ impl PhysicsState{
 	fn next_move_instruction(&self)->Option<TimedInstruction<InternalInstruction,Time>>{
 		self.move_state.next_move_instruction(&self.style.strafe,self.time)
 	}
-	fn cull_velocity(&mut self,data:&PhysicsData,velocity:Planar64Vec3){
-		self.move_state.cull_velocity(velocity,&mut self.body,&mut self.touching,&data.models,&data.hitbox_mesh,&self.style,&self.camera,&self.input_state);
-	}
 	fn set_move_state(&mut self,data:&PhysicsData,move_state:MoveState){
 		self.move_state.set_move_state(move_state,&mut self.body,&self.touching,&data.models,&data.hitbox_mesh,&self.style,&self.camera,&self.input_state);
 	}
@@ -1329,22 +1348,50 @@ fn set_position(
 	recalculate_touching(move_state,body,touching,run,mode_state,mode,models,hitbox_mesh,bvh,style,camera,input_state,time);
 	point
 }
+/// Returns true when a contact was removed
 fn set_velocity_cull(body:&mut Body,touching:&mut TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,v:Planar64Vec3)->bool{
-	//This is not correct but is better than what I have
-	let mut culled=false;
-	touching.contacts.retain(|convex_mesh_id,face_id|{
-		let n=contact_normal(models,hitbox_mesh,convex_mesh_id,*face_id);
-		let r=n.dot(v).is_positive();
-		if r{
-			culled=true;
-		}
-		!r
+	let contacts=touching.contacts(models,hitbox_mesh);
+	let ConstrainedVelocity{velocity,mut constraints}=touching.constrain_velocity(&contacts,v);
+
+	// prep list for drain
+	constraints.sort_by_key(|&cont|{
+		let cont_ptr:*const crate::push_solve::Contact=cont;
+		contacts.contacts.len()-(cont_ptr as usize-contacts.contacts.as_ptr() as usize)
 	});
-	set_velocity(body,touching,models,hitbox_mesh,v);
-	culled
+	// create a list of indices to retain
+	let mut indices:arrayvec::ArrayVec<_,4>=constraints.iter().map(|&cont|{
+		let cont_ptr:*const crate::push_solve::Contact=cont;
+		cont_ptr as usize-contacts.contacts.as_ptr() as usize
+	}).collect();
+
+	drop(constraints);
+
+	let mut is_contact_removed=false;
+
+	// Delete contacts which do not constrain the velocity
+	let mut i=0;
+	touching.contacts.retain(|_,_|{
+		if let Some(&next_i)=indices.last(){
+			let is_active=i==next_i;
+			if is_active{
+				indices.pop();
+			}else{
+				is_contact_removed=true;
+			}
+			i+=1;
+			return is_active
+		}
+		is_contact_removed=true;
+		false
+	});
+
+	body.velocity=velocity;
+
+	is_contact_removed
 }
 fn set_velocity(body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,v:Planar64Vec3){
-	body.velocity=touching.constrain_velocity(models,hitbox_mesh,v);
+	let contacts=touching.contacts(models,hitbox_mesh);
+	body.velocity=touching.constrain_velocity(&contacts,v).velocity;
 }
 
 fn teleport(
@@ -1703,20 +1750,7 @@ fn collision_end_contact(
 	//check ground
 	//TODO do better
 	//this is inner code from move_state.cull_velocity
-	match move_state.get_walk_state(){
-		// did you stop touching the thing you were walking on?
-		// This does not check the face! Is that a bad thing? It should be
-		// impossible to stop touching a different face than you started touching...
-		Some(walk_state)=>if &walk_state.contact.convex_mesh_id==convex_mesh_id{
-			move_state.set_move_state(MoveState::Air,body,touching,models,hitbox_mesh,style,camera,input_state);
-		}else{
-			// stopped touching something else while walking
-			move_state.update_walk_target(body,touching,models,hitbox_mesh,style,camera,input_state);
-			move_state.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state);
-		},
-		// not walking, but stopped touching something
-		None=>move_state.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state),
-	}
+	move_state.touching_changed(body,touching,models,hitbox_mesh,style,camera,input_state,|contact|contact.convex_mesh_id==*convex_mesh_id);
 }
 fn collision_end_intersect(
 	move_state:&mut MoveState,
@@ -1813,7 +1847,7 @@ fn atomic_internal_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:Tim
 							state.body=extrapolated_body;
 							//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);
+							state.move_state.cull_velocity(ticked_velocity,&mut state.body,&mut state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state);
 						}
 					}
 				}
@@ -1897,7 +1931,7 @@ fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedI
 					let jump_dir=walk_state.jump_direction.direction(&data.models,&data.hitbox_mesh,&walk_state.contact);
 					let booster_option=data.models.contact_attr(walk_state.contact.convex_mesh_id.model_id).general.booster.as_ref();
 					let jumped_velocity=jump_settings.jumped_velocity(&state.style,jump_dir,state.body.velocity,booster_option);
-					state.cull_velocity(data,jumped_velocity);
+					state.move_state.cull_velocity(jumped_velocity,&mut state.body,&mut state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state);
 				}
 			}
 			b_refresh_walk_target=false;
@@ -1970,7 +2004,7 @@ fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedI
 	if b_refresh_walk_target{
 		state.move_state.update_walk_target(&mut state.body,&state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state);
 		state.move_state.update_fly_velocity(&mut state.body,&state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state);
-		state.cull_velocity(data,state.body.velocity);
+		state.move_state.cull_velocity(state.body.velocity,&mut state.body,&mut state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state);
 		//also check if accelerating away from surface
 	}
 }

engine/physics/src/push_solve.rs

@@ -10,7 +10,7 @@ use strafesnet_common::ray::Ray;
 
 // 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>;
+pub type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>;
 
 // hack to allow comparing ratios to zero
 const RATIO_ZERO:Ratio<F64_32,F64_32>=Ratio::new(Fixed::ZERO,Fixed::EPSILON);