From b4abe437f656940f6c7502b7f73beb003941f728 Mon Sep 17 00:00:00 2001
From: Quaternions <krakow20@gmail.com>
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<Ray>{
Some(Ray{origin:point,direction:Planar64Vec3::ZERO})
}
@@ -113,7 +112,7 @@ fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option<Ray>{
)?;
Some(Ray{origin,direction})
}
-fn get_best_push_ray_and_indices_1(pnv_list:&Vec<Contact>,point:Planar64Vec3,i0:usize)->Option<(Ray,Indices)>{
+fn get_best_push_ray_and_indices_1(point:Planar64Vec3,pnv_list:&Vec<Contact>,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<Ray>{
)?;
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)>{
+fn get_best_push_ray_and_indices_2(point:Planar64Vec3,pnv_list:&Vec<Contact>,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<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_3(point:Planar64Vec3,pnv_list:&Vec<Contact>,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<Contact>,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<Contact>,
point:Planar64Vec3,
+ pnv_list:&Vec<Contact>,
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<Contact>,point:Planar64Vec3){