From 1928c2fc40ba75575bbd528932fa8fd776d1e917 Mon Sep 17 00:00:00 2001
From: Quaternions <krakow20@gmail.com>
Date: Wed, 21 Aug 2024 16:54:02 -0700
Subject: [PATCH] wip

---
 src/main.rs       |   1 +
 src/push_solve.rs | 182 ++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 183 insertions(+)
 create mode 100644 src/push_solve.rs

diff --git a/src/main.rs b/src/main.rs
index 337ba79..d779781 100644
--- a/src/main.rs
+++ b/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;
diff --git a/src/push_solve.rs b/src/push_solve.rs
new file mode 100644
index 0000000..dc73c55
--- /dev/null
+++ b/src/push_solve.rs
@@ -0,0 +1,182 @@
+use strafesnet_common::integer::{Planar64,Planar64Vec3};
+
+type Indices=arrayvec::ArrayVec<usize,4>;
+
+struct Ray{
+	origin:Planar64Vec3,
+	direction:Planar64Vec3,
+}
+
+/// Information about a contact restriction
+pub struct Contact{
+	pub position:Planar64Vec3,
+	pub velocity:Planar64Vec3,
+	pub normal:Planar64Vec3,
+}
+impl Contact{
+	const fn relative_to(&self,point:Planar64Vec3)->Self{
+		Self{
+			position:self.position-point,
+			velocity:self.velocity,
+			normal:self.normal,
+		}
+	}
+}
+
+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.get().abs()<0{
+		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)
+}
+
+fn test_positivity(direction:Planar64Vec3,c:&Contact)->bool{
+	Planar64::ZERO<=(direction-c.velocity).dot(c.normal)
+}
+
+const fn get_push_ray_0(point:Planar64Vec3)->Option<Ray>{
+	Some(Ray{origin:point,direction:Planar64Vec3::ZERO})
+}
+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_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_best_push_ray_and_indices_1(pnv_list:&Vec<Contact>,point:Planar64Vec3,i0:usize)->Option<(Ray,Indices)>{
+	get_push_ray_1(point,&pnv_list[i0])
+		.map(|ray|(ray,Indices::from_iter([i0])))
+}
+
+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_best_push_ray_and_indices_2(pnv_list:&Vec<Contact>,point:Planar64Vec3,i0:usize,i1:usize)->Option<(Ray,Indices)>{
+	if let Some(ray)=get_push_ray_2(point,&pnv_list[i0],&pnv_list[i1]){
+		return Some((ray,Indices::from_iter([i0,i1])));
+	}
+	if let Some(ray)=get_push_ray_1(point,&pnv_list[i0]){
+		if test_positivity(ray.direction,&pnv_list[i0]){
+			return Some((ray,Indices::from_iter([i0])));
+		}
+	}
+	return None;
+}
+
+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})
+}
+fn get_best_push_ray_and_indices_3(pnv_list:&Vec<Contact>,point:Planar64Vec3,i0:usize,i1:usize,i2:usize)->Option<(Ray,Indices)>{
+	get_push_ray_3(point,&pnv_list[i0],&pnv_list[i1],&pnv_list[i2])
+		.map(|ray|(ray,Indices::from_iter([i0,i1,i2])))
+}
+
+fn get_best_push_ray_and_indices(
+	pnv_list:&Vec<Contact>,
+	point:Planar64Vec3,
+	indices:Indices,
+)->Option<(Ray,Indices)>{
+	let is_space_enclosed=match indices.as_slice(){
+		&[i0,i1,i2,i3]=>is_space_enclosed_4(pnv_list[i0].normal,pnv_list[i1].normal,pnv_list[i2].normal,pnv_list[i3].normal),
+		&[i0,i1,i2]=>is_space_enclosed_3(pnv_list[i0].normal,pnv_list[i1].normal,pnv_list[i2].normal),
+		&[i0,i1]=>is_space_enclosed_2(pnv_list[i0].normal,pnv_list[i1].normal),
+		&[_]|&[]=>is_space_enclosed_0_1(),
+	};
+}
+
+pub fn push_solve(pnv_list:&Vec<Contact>,point:Planar64Vec3){
+	//
+}
+
+pub fn push_solve_with_output(pnv_list:&Vec<Contact>,point:Planar64Vec3,output:&mut Vec<Planar64Vec3>){
+	//
+}