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; + +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{ + 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)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_abdbool{ + Planar64::ZERO<=(direction-c.velocity).dot(c.normal) +} + +const fn get_push_ray_0(point:Planar64Vec3)->Option{ + 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{ + let direction=solve1(c0)?; + let s0=decompose1(direction,c0.velocity)?; + if s0,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{ + let direction=solve2(c0,c1)?; + let (s0,s1)=decompose2(direction,c0.velocity,c1.velocity)?; + if s0,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{ + let direction=solve3(c0,c1,c2)?; + let (s0,s1,s2)=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?; + if s0,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, + 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,point:Planar64Vec3){ + // +} + +pub fn push_solve_with_output(pnv_list:&Vec,point:Planar64Vec3,output:&mut Vec){ + // +}