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)
@@ -753,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
@@ -808,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(),
@@ -817,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);
@@ -1327,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(

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);