strafe-project/src/push_solve.rs

use strafesnet_common::integer::{Planar64,Planar64Vec3};

const EPSILON:Planar64=Planar64::raw(1<<(32-10));

type Indices=arrayvec::ArrayVec<usize,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)
	}
}
#[derive(Clone,Copy)]
struct CI<'a>{
	contact:&'a Contact,
	index:usize,
}
impl CI<'_>{
	fn new(pnv_list:&Vec<Contact>,index:usize)->CI<'_>{
		CI{
			contact:&pnv_list[index],
			index
		}
	}
}

const fn solve0()->Planar64Vec3{
	Planar64Vec3::ZERO
}

fn solve1(c0:&Contact)->Option<Planar64Vec3>{
	let d0=c0.normal.dot(c0.position);
	let det=c0.normal.dot(c0.velocity);
	if det.abs()<EPSILON{
		None
	}else{
		Some(c0.normal*d0/det)
	}
}

const fn is_space_enclosed_0_1()->bool{
	false
}

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_indices_0(point:Planar64Vec3)->Option<(Ray,Indices)>{
	match get_push_ray_0(point){
		Some(ray)=>Some((ray,Indices::new_const())),
		None=>None,
	}
}
fn get_best_push_ray_and_indices_1(point:Planar64Vec3,ci0:CI)->Option<(Ray,Indices)>{
	get_push_ray_1(point,ci0.contact)
		.map(|ray|(ray,Indices::from_iter([ci0.index])))
}
fn get_best_push_ray_and_indices_2(point:Planar64Vec3,ci0:CI,ci1:CI)->Option<(Ray,Indices)>{
	if is_space_enclosed_2(ci0.contact.normal,ci1.contact.normal){
		return None;
	}
	if let Some(ray)=get_push_ray_2(point,ci0.contact,ci1.contact){
		return Some((ray,Indices::from_iter([ci0.index,ci1.index])));
	}
	if let Some(ray)=get_push_ray_1(point,ci0.contact){
		if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){
			return Some((ray,Indices::from_iter([ci0.index])));
		}
	}
	return None;
}
fn get_best_push_ray_and_indices_3(point:Planar64Vec3,ci0:CI,ci1:CI,ci2:CI)->Option<(Ray,Indices)>{
	if is_space_enclosed_3(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal){
		return None;
	}
	if let Some(ray)=get_push_ray_3(point,ci0.contact,ci1.contact,ci2.contact){
		return Some((ray,Indices::from_iter([ci0.index,ci1.index,ci2.index])));
	}
	if let Some(ray)=get_push_ray_2(point,ci0.contact,ci1.contact){
		if Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){
			return Some((ray,Indices::from_iter([ci0.index,ci1.index])));
		}
	}
	if let Some(ray)=get_push_ray_2(point,ci0.contact,ci2.contact){
		if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){
			return Some((ray,Indices::from_iter([ci0.index,ci2.index])));
		}
	}
	if let Some(ray)=get_push_ray_1(point,ci0.contact){
		if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction)
		 &&Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){
			return Some((ray,Indices::from_iter([ci0.index])));
		}
	}
	return None;
}
fn get_best_push_ray_and_indices_4(point:Planar64Vec3,ci0:CI,ci1:CI,ci2:CI,ci3:CI)->Option<(Ray,Indices)>{
	if is_space_enclosed_4(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal,ci3.contact.normal){
		return None;
	}

	let (ray012,indices012)=get_best_push_ray_and_indices_3(point,ci0,ci1,ci2)?;
	let (ray013,indices013)=get_best_push_ray_and_indices_3(point,ci0,ci1,ci3)?;
	let (ray023,indices023)=get_best_push_ray_and_indices_3(point,ci0,ci2,ci3)?;

	let err012=ci3.contact.relative_dot(ray012.direction);
	let err013=ci2.contact.relative_dot(ray013.direction);
	let err023=ci1.contact.relative_dot(ray023.direction);

	let best_err=err012.max(err013).max(err023);

	if best_err==err012{
		return Some((ray012,indices012))
	}else if best_err==err012{
		return Some((ray013,indices013))
	}else if best_err==err012{
		return Some((ray023,indices023))
	}
	unreachable!()
}

fn get_best_push_ray_and_indices(
	point:Planar64Vec3,
	pnv_list:&Vec<Contact>,
	indices:Indices,
)->Option<(Ray,Indices)>{
	match indices.as_slice(){
		&[i0,i1,i2,i3]=>get_best_push_ray_and_indices_4(point,CI::new(pnv_list,i0),CI::new(pnv_list,i1),CI::new(pnv_list,i2),CI::new(pnv_list,i3)),
		&[i0,i1,i2]=>get_best_push_ray_and_indices_3(point,CI::new(pnv_list,i0),CI::new(pnv_list,i1),CI::new(pnv_list,i2)),
		&[i0,i1]=>get_best_push_ray_and_indices_2(point,CI::new(pnv_list,i0),CI::new(pnv_list,i1)),
		&[i0]=>get_best_push_ray_and_indices_1(point,CI::new(pnv_list,i0)),
		&[]=>get_best_push_ray_and_indices_0(point),
		_=>unreachable!(),
	}
}

pub fn push_solve(pnv_list:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
	let (mut ray,mut indices)=get_best_push_ray_and_indices_0(point)?;
	loop{
		let (next_index,next_t)=get_first_touch(pnv_list,ray,indices)?;

		if Planar64::ZERO<=next_t{
			return Some(ray.origin);
		}

		//edit the indices
		if indices.len()==4{
			indices.pop();
		}
		indices.push(next_index);

		let meet_point=ray.extrapolate(next_t);
		match get_best_push_ray_and_indices(meet_point,pnv_list,indices){
			Some((new_ray,new_indices))=>(ray,indices)=(new_ray,new_indices),
			None=>return Some(meet_point),
		}
	}
}

pub fn push_solve_with_output(pnv_list:&Vec<Contact>,point:Planar64Vec3,output:&mut Vec<Planar64Vec3>){
	//
}