From b4abe437f656940f6c7502b7f73beb003941f728 Mon Sep 17 00:00:00 2001 From: Quaternions Date: Wed, 21 Aug 2024 17:38:48 -0700 Subject: [PATCH] wip --- src/push_solve.rs | 86 +++++++++++++++++++++++++++++++++++++---------- 1 file changed, 68 insertions(+), 18 deletions(-) diff --git a/src/push_solve.rs b/src/push_solve.rs index dc73c55..e0d0283 100644 --- a/src/push_solve.rs +++ b/src/push_solve.rs @@ -14,13 +14,16 @@ pub struct Contact{ pub normal:Planar64Vec3, } impl Contact{ - const fn relative_to(&self,point:Planar64Vec3)->Self{ + 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) + } } const fn solve0()->Planar64Vec3{ @@ -88,10 +91,6 @@ fn is_space_enclosed_4( ||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{ Some(Ray{origin:point,direction:Planar64Vec3::ZERO}) } @@ -113,7 +112,7 @@ fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option{ )?; Some(Ray{origin,direction}) } -fn get_best_push_ray_and_indices_1(pnv_list:&Vec,point:Planar64Vec3,i0:usize)->Option<(Ray,Indices)>{ +fn get_best_push_ray_and_indices_1(point:Planar64Vec3,pnv_list:&Vec,i0:usize)->Option<(Ray,Indices)>{ get_push_ray_1(point,&pnv_list[i0]) .map(|ray|(ray,Indices::from_iter([i0]))) } @@ -130,12 +129,15 @@ fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option{ )?; Some(Ray{origin,direction}) } -fn get_best_push_ray_and_indices_2(pnv_list:&Vec,point:Planar64Vec3,i0:usize,i1:usize)->Option<(Ray,Indices)>{ +fn get_best_push_ray_and_indices_2(point:Planar64Vec3,pnv_list:&Vec,i0:usize,i1:usize)->Option<(Ray,Indices)>{ + if is_space_enclosed_2(pnv_list[i0].normal,pnv_list[i1].normal){ + return None; + } 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]){ + if Planar64::ZERO<=pnv_list[i1].relative_dot(ray.direction){ return Some((ray,Indices::from_iter([i0]))); } } @@ -155,22 +157,70 @@ fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Optio )?; Some(Ray{origin,direction}) } -fn get_best_push_ray_and_indices_3(pnv_list:&Vec,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_3(point:Planar64Vec3,pnv_list:&Vec,i0:usize,i1:usize,i2:usize)->Option<(Ray,Indices)>{ + if is_space_enclosed_3(pnv_list[i0].normal,pnv_list[i1].normal,pnv_list[i2].normal){ + return None; + } + if let Some(ray)=get_push_ray_3(point,&pnv_list[i0],&pnv_list[i1],&pnv_list[i2]){ + return Some((ray,Indices::from_iter([i0,i1,i2]))); + } + if let Some(ray)=get_push_ray_2(point,&pnv_list[i0],&pnv_list[i1]){ + if Planar64::ZERO<=pnv_list[i2].relative_dot(ray.direction){ + return Some((ray,Indices::from_iter([i0,i1]))); + } + } + if let Some(ray)=get_push_ray_2(point,&pnv_list[i0],&pnv_list[i2]){ + if Planar64::ZERO<=pnv_list[i1].relative_dot(ray.direction){ + return Some((ray,Indices::from_iter([i0,i2]))); + } + } + if let Some(ray)=get_push_ray_1(point,&pnv_list[i0]){ + if Planar64::ZERO<=pnv_list[i1].relative_dot(ray.direction) + &&Planar64::ZERO<=pnv_list[i2].relative_dot(ray.direction){ + return Some((ray,Indices::from_iter([i0]))); + } + } + return None; +} + +fn get_best_push_ray_and_indices_4(point:Planar64Vec3,pnv_list:&Vec,i0:usize,i1:usize,i2:usize,i3:usize)->Option<(Ray,Indices)>{ + if is_space_enclosed_4(pnv_list[i0].normal,pnv_list[i1].normal,pnv_list[i2].normal,pnv_list[i3].normal){ + return None; + } + + let (ray012,indices012)=get_best_push_ray_and_indices_3(point,pnv_list,i0,i1,i2)?; + let (ray013,indices013)=get_best_push_ray_and_indices_3(point,pnv_list,i0,i1,i3)?; + let (ray023,indices023)=get_best_push_ray_and_indices_3(point,pnv_list,i0,i2,i3)?; + + let err012=pnv_list[i3].relative_dot(ray012.direction); + let err013=pnv_list[i2].relative_dot(ray013.direction); + let err023=pnv_list[i1].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( - pnv_list:&Vec, point:Planar64Vec3, + pnv_list:&Vec, 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(), - }; + match indices.as_slice(){ + &[i0,i1,i2,i3]=>get_best_push_ray_and_indices_4(point,pnv_list,i0,i1,i2,i3), + &[i0,i1,i2]=>get_best_push_ray_and_indices_3(point,pnv_list,i0,i1,i2), + &[i0,i1]=>get_best_push_ray_and_indices_2(point,pnv_list,i0,i1), + &[i0]=>get_best_push_ray_and_indices_1(point,pnv_list,i0), + &[]=>get_best_push_ray_and_indices_0(point), + _=>unreachable!(), + } } pub fn push_solve(pnv_list:&Vec,point:Planar64Vec3){