StrafesNET/strafe-client
5
Fork 4
Code Issues 2 Pull Requests 1 Packages Projects Releases 1 Wiki Activity

data structure for contact with index

Browse Source
This commit is contained in:
Quaternions 2024-08-21 17:53:07 -07:00
parent b4abe437f6
commit 412d9ada8f
1 changed files with 49 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
src/push_solve.rs
View File

@ -25,6 +25,19 @@ impl Contact{
(direction-self.velocity).dot(self.normal) (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{ const fn solve0()->Planar64Vec3{
Planar64Vec3::ZERO Planar64Vec3::ZERO
@ -112,9 +125,9 @@ fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option<Ray>{
)?; )?;
Some(Ray{origin,direction}) Some(Ray{origin,direction})
} }
fn get_best_push_ray_and_indices_1(point:Planar64Vec3,pnv_list:&Vec<Contact>,i0:usize)->Option<(Ray,Indices)>{ fn get_best_push_ray_and_indices_1(point:Planar64Vec3,ci0:CI)->Option<(Ray,Indices)>{
get_push_ray_1(point,&pnv_list[i0]) get_push_ray_1(point,ci0.contact)
.map(|ray|(ray,Indices::from_iter([i0]))) .map(|ray|(ray,Indices::from_iter([ci0.index])))
} }
fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option<Ray>{ fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option<Ray>{
@ -129,16 +142,16 @@ fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option<Ray>{
)?; )?;
Some(Ray{origin,direction}) Some(Ray{origin,direction})
} }
fn get_best_push_ray_and_indices_2(point:Planar64Vec3,pnv_list:&Vec<Contact>,i0:usize,i1:usize)->Option<(Ray,Indices)>{ fn get_best_push_ray_and_indices_2(point:Planar64Vec3,ci0:CI,ci1:CI)->Option<(Ray,Indices)>{
if is_space_enclosed_2(pnv_list[i0].normal,pnv_list[i1].normal){ if is_space_enclosed_2(ci0.contact.normal,ci1.contact.normal){
return None; return None;
} }
if let Some(ray)=get_push_ray_2(point,&pnv_list[i0],&pnv_list[i1]){ if let Some(ray)=get_push_ray_2(point,ci0.contact,ci1.contact){
return Some((ray,Indices::from_iter([i0,i1]))); return Some((ray,Indices::from_iter([ci0.index,ci1.index])));
} }
if let Some(ray)=get_push_ray_1(point,&pnv_list[i0]){ if let Some(ray)=get_push_ray_1(point,ci0.contact){
if Planar64::ZERO<=pnv_list[i1].relative_dot(ray.direction){ if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){
return Some((ray,Indices::from_iter([i0]))); return Some((ray,Indices::from_iter([ci0.index])));
} }
} }
return None; return None;
@ -157,44 +170,44 @@ fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Optio
)?; )?;
Some(Ray{origin,direction}) Some(Ray{origin,direction})
} }
fn get_best_push_ray_and_indices_3(point:Planar64Vec3,pnv_list:&Vec<Contact>,i0:usize,i1:usize,i2:usize)->Option<(Ray,Indices)>{ fn get_best_push_ray_and_indices_3(point:Planar64Vec3,ci0:CI,ci1:CI,ci2:CI)->Option<(Ray,Indices)>{
if is_space_enclosed_3(pnv_list[i0].normal,pnv_list[i1].normal,pnv_list[i2].normal){ if is_space_enclosed_3(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal){
return None; return None;
} }
if let Some(ray)=get_push_ray_3(point,&pnv_list[i0],&pnv_list[i1],&pnv_list[i2]){ if let Some(ray)=get_push_ray_3(point,ci0.contact,ci1.contact,ci2.contact){
return Some((ray,Indices::from_iter([i0,i1,i2]))); return Some((ray,Indices::from_iter([ci0.index,ci1.index,ci2.index])));
} }
if let Some(ray)=get_push_ray_2(point,&pnv_list[i0],&pnv_list[i1]){ if let Some(ray)=get_push_ray_2(point,ci0.contact,ci1.contact){
if Planar64::ZERO<=pnv_list[i2].relative_dot(ray.direction){ if Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){
return Some((ray,Indices::from_iter([i0,i1]))); return Some((ray,Indices::from_iter([ci0.index,ci1.index])));
} }
} }
if let Some(ray)=get_push_ray_2(point,&pnv_list[i0],&pnv_list[i2]){ if let Some(ray)=get_push_ray_2(point,ci0.contact,ci2.contact){
if Planar64::ZERO<=pnv_list[i1].relative_dot(ray.direction){ if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){
return Some((ray,Indices::from_iter([i0,i2]))); return Some((ray,Indices::from_iter([ci0.index,ci2.index])));
} }
} }
if let Some(ray)=get_push_ray_1(point,&pnv_list[i0]){ if let Some(ray)=get_push_ray_1(point,ci0.contact){
if Planar64::ZERO<=pnv_list[i1].relative_dot(ray.direction) if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction)
&&Planar64::ZERO<=pnv_list[i2].relative_dot(ray.direction){ &&Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){
return Some((ray,Indices::from_iter([i0]))); return Some((ray,Indices::from_iter([ci0.index])));
} }
} }
return None; 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)>{ 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(pnv_list[i0].normal,pnv_list[i1].normal,pnv_list[i2].normal,pnv_list[i3].normal){ if is_space_enclosed_4(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal,ci3.contact.normal){
return None; return None;
} }
let (ray012,indices012)=get_best_push_ray_and_indices_3(point,pnv_list,i0,i1,i2)?; 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,pnv_list,i0,i1,i3)?; 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,pnv_list,i0,i2,i3)?; let (ray023,indices023)=get_best_push_ray_and_indices_3(point,ci0,ci2,ci3)?;
let err012=pnv_list[i3].relative_dot(ray012.direction); let err012=ci3.contact.relative_dot(ray012.direction);
let err013=pnv_list[i2].relative_dot(ray013.direction); let err013=ci2.contact.relative_dot(ray013.direction);
let err023=pnv_list[i1].relative_dot(ray023.direction); let err023=ci1.contact.relative_dot(ray023.direction);
let best_err=err012.max(err013).max(err023); let best_err=err012.max(err013).max(err023);
@ -214,10 +227,10 @@ fn get_best_push_ray_and_indices(
indices:Indices, indices:Indices,
)->Option<(Ray,Indices)>{ )->Option<(Ray,Indices)>{
match indices.as_slice(){ 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,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,pnv_list,i0,i1,i2), &[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,pnv_list,i0,i1), &[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,pnv_list,i0), &[i0]=>get_best_push_ray_and_indices_1(point,CI::new(pnv_list,i0)),
&[]=>get_best_push_ray_and_indices_0(point), &[]=>get_best_push_ray_and_indices_0(point),
_=>unreachable!(), _=>unreachable!(),
} }