strafe-project/strafe-client/src/face_crawler.rs

use crate::physics::Body;
use crate::model_physics::{GigaTime,FEV,MeshQuery,DirectedEdge,MinkowskiMesh,MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert};
use strafesnet_common::integer::{Time,Fixed,Ratio};

#[derive(Debug)]
enum Transition<F,E:DirectedEdge,V>{
	Miss,
	Next(FEV<F,E,V>,GigaTime),
	Hit(F,GigaTime),
}

type MinkowskiFEV=FEV<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>;
type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>;

	fn next_transition(fev:&MinkowskiFEV,body_time:GigaTime,mesh:&MinkowskiMesh,body:&Body,mut best_time:GigaTime)->MinkowskiTransition{
		//conflicting derivative means it crosses in the wrong direction.
		//if the transition time is equal to an already tested transition, do not replace the current best.
		let mut best_transition=MinkowskiTransition::Miss;
		match fev{
			&MinkowskiFEV::Face(face_id)=>{
				//test own face collision time, ignoring roots with zero or conflicting derivative
				//n=face.normal d=face.dot
				//n.a t^2+n.v t+n.p-d==0
				let (n,d)=mesh.face_nd(face_id);
				//TODO: use higher precision d value?
				//use the mesh transform translation instead of baking it into the d value.
				for dt in Fixed::<4,128>::zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
					if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
						best_time=dt;
						best_transition=MinkowskiTransition::Hit(face_id,dt);
						break;
					}
				}
				//test each edge collision time, ignoring roots with zero or conflicting derivative
				for &directed_edge_id in mesh.face_edges(face_id).iter(){
					let edge_n=mesh.directed_edge_n(directed_edge_id);
					let n=n.cross(edge_n);
					let verts=mesh.edge_verts(directed_edge_id.as_undirected());
					//WARNING: d is moved out of the *2 block because of adding two vertices!
					//WARNING: precision is swept under the rug!
					for dt in Fixed::<4,128>::zeroes2(n.dot(body.position*2-(mesh.vert(verts[0])+mesh.vert(verts[1]))).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){
						if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
							best_time=dt;
							best_transition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt);
							break;
						}
					}
				}
				//if none:
			},
			&MinkowskiFEV::Edge(edge_id)=>{
				//test each face collision time, ignoring roots with zero or conflicting derivative
				let edge_n=mesh.edge_n(edge_id);
				let edge_verts=mesh.edge_verts(edge_id);
				let delta_pos=body.position*2-(mesh.vert(edge_verts[0])+mesh.vert(edge_verts[1]));
				for (i,&edge_face_id) in mesh.edge_faces(edge_id).iter().enumerate(){
					let face_n=mesh.face_nd(edge_face_id).0;
					//edge_n gets parity from the order of edge_faces
					let n=face_n.cross(edge_n)*((i as i64)*2-1);
					//WARNING yada yada d *2
					for dt in Fixed::<4,128>::zeroes2(n.dot(delta_pos).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){
						if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
							best_time=dt;
							best_transition=MinkowskiTransition::Next(MinkowskiFEV::Face(edge_face_id),dt);
							break;
						}
					}
				}
				//test each vertex collision time, ignoring roots with zero or conflicting derivative
				for (i,&vert_id) in edge_verts.iter().enumerate(){
					//vertex normal gets parity from vert index
					let n=edge_n*(1-2*(i as i64));
					for dt in Fixed::<2,64>::zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
						if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
							let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4());
							best_time=dt;
							best_transition=MinkowskiTransition::Next(MinkowskiFEV::Vert(vert_id),dt);
							break;
						}
					}
				}
				//if none:
			},
			&MinkowskiFEV::Vert(vert_id)=>{
				//test each edge collision time, ignoring roots with zero or conflicting derivative
				for &directed_edge_id in mesh.vert_edges(vert_id).iter(){
					//edge is directed away from vertex, but we want the dot product to turn out negative
					let n=-mesh.directed_edge_n(directed_edge_id);
					for dt in Fixed::<2,64>::zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
						if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
							let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4());
							best_time=dt;
							best_transition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt);
							break;
						}
					}
				}
				//if none:
			},
		}
		best_transition
	}
pub enum CrawlResult<F,E:DirectedEdge,V>{
	Miss(FEV<F,E,V>),
	Hit(F,GigaTime),
}
type MinkowskiCrawlResult=CrawlResult<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>;
pub fn crawl_fev(mut fev:MinkowskiFEV,mesh:&MinkowskiMesh,relative_body:&Body,start_time:Time,time_limit:Time)->MinkowskiCrawlResult{
	let mut body_time={
		let r=(start_time-relative_body.time).to_ratio();
		Ratio::new(r.num.fix_4(),r.den.fix_4())
	};
	let time_limit={
		let r=(time_limit-relative_body.time).to_ratio();
		Ratio::new(r.num.fix_4(),r.den.fix_4())
	};
	for _ in 0..20{
		match next_transition(&fev,body_time,mesh,relative_body,time_limit){
			Transition::Miss=>return CrawlResult::Miss(fev),
			Transition::Next(next_fev,next_time)=>(fev,body_time)=(next_fev,next_time),
			Transition::Hit(face,time)=>return CrawlResult::Hit(face,time),
		}
	}
	//TODO: fix all bugs
	//println!("Too many iterations!  Using default behaviour instead of crashing...");
	CrawlResult::Miss(fev)
}