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

tentative algorithm completion

Browse Source
This commit is contained in:
Quaternions 2024-08-21 19:18:01 -07:00
parent 24a7fce2a9
commit 94d8bcfcea
1 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/push_solve.rs
View File

@ -286,10 +286,24 @@ fn get_best_push_ray_and_indices(
} }
} }
fn get_touch_time(ray:&Ray,c:&Contact)->Planar64{
(ray.origin-c.position).dot(c.normal)/(c.velocity-ray.direction).dot(c.normal)
}
fn get_first_touch(pnv_list:&Vec<Contact>,ray:&Ray,indices:&Indices)->Option<(Planar64,usize)>{
pnv_list.iter()
.enumerate()
.filter(|&(i,contact)|
!indices.contains(&i)
&&contact.relative_dot(ray.direction)< -EPSILON
)
.map(|(i,contact)|(get_touch_time(ray,contact),i))
.min_by_key(|&(t,_)|t)
}
pub fn push_solve(pnv_list:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{ pub fn push_solve(pnv_list:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
let (mut ray,mut indices)=get_best_push_ray_and_indices_0(point)?; let (mut ray,mut indices)=get_best_push_ray_and_indices_0(point)?;
loop{ loop{
let (next_index,next_t)=get_first_touch(pnv_list,ray,indices)?; let (next_t,next_index)=get_first_touch(pnv_list,&ray,&indices)?;
if Planar64::ZERO<=next_t{ if Planar64::ZERO<=next_t{
return Some(ray.origin); return Some(ray.origin);