factor out indices, use only direct references
This commit is contained in:
parent
1265926c98
commit
78202beb53
@ -4,8 +4,8 @@ use strafesnet_common::integer::{Planar64,Planar64Vec3};
|
||||
const EPSILON:Planar64=Planar64::raw(1<<(32-10));
|
||||
|
||||
// A stack-allocated variable-size list that holds up to 4 elements
|
||||
// This is used for i0, i1, i2, i3
|
||||
type Indices=arrayvec::ArrayVec<usize,4>;
|
||||
// Direct references are used instead of indices i0, i1, i2, i3
|
||||
type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>;
|
||||
|
||||
struct Ray{
|
||||
origin:Planar64Vec3,
|
||||
@ -40,21 +40,6 @@ impl Contact{
|
||||
}
|
||||
}
|
||||
|
||||
//A reference to a Contact and its respective index, paired together
|
||||
#[derive(Clone,Copy)]
|
||||
struct CI<'a>{
|
||||
contact:&'a Contact,
|
||||
index:usize,
|
||||
}
|
||||
impl CI<'_>{
|
||||
fn new(conts:&Vec<Contact>,index:usize)->CI<'_>{
|
||||
CI{
|
||||
contact:&conts[index],
|
||||
index
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//note that this is horrible with fixed point arithmetic
|
||||
fn solve1(c0:&Contact)->Option<Planar64Vec3>{
|
||||
let det=c0.normal.dot(c0.velocity);
|
||||
@ -203,124 +188,122 @@ fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Optio
|
||||
Some(Ray{origin,direction})
|
||||
}
|
||||
|
||||
const fn get_best_push_ray_and_indices_0(point:Planar64Vec3)->Option<(Ray,Indices)>{
|
||||
const fn get_best_push_ray_and_conts_0<'a>(point:Planar64Vec3)->Option<(Ray,Conts<'a>)>{
|
||||
match get_push_ray_0(point){
|
||||
Some(ray)=>Some((ray,Indices::new_const())),
|
||||
Some(ray)=>Some((ray,Conts::new_const())),
|
||||
None=>None,
|
||||
}
|
||||
}
|
||||
fn get_best_push_ray_and_indices_1(point:Planar64Vec3,ci0:CI)->Option<(Ray,Indices)>{
|
||||
get_push_ray_1(point,ci0.contact)
|
||||
.map(|ray|(ray,Indices::from_iter([ci0.index])))
|
||||
fn get_best_push_ray_and_conts_1(point:Planar64Vec3,c0:&Contact)->Option<(Ray,Conts)>{
|
||||
get_push_ray_1(point,c0)
|
||||
.map(|ray|(ray,Conts::from_iter([c0])))
|
||||
}
|
||||
fn get_best_push_ray_and_indices_2(point:Planar64Vec3,ci0:CI,ci1:CI)->Option<(Ray,Indices)>{
|
||||
if is_space_enclosed_2(ci0.contact.normal,ci1.contact.normal){
|
||||
fn get_best_push_ray_and_conts_2<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact)->Option<(Ray,Conts<'a>)>{
|
||||
if is_space_enclosed_2(c0.normal,c1.normal){
|
||||
return None;
|
||||
}
|
||||
if let Some(ray)=get_push_ray_2(point,ci0.contact,ci1.contact){
|
||||
return Some((ray,Indices::from_iter([ci0.index,ci1.index])));
|
||||
if let Some(ray)=get_push_ray_2(point,c0,c1){
|
||||
return Some((ray,Conts::from_iter([c0,c1])));
|
||||
}
|
||||
if let Some(ray)=get_push_ray_1(point,ci0.contact){
|
||||
if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){
|
||||
return Some((ray,Indices::from_iter([ci0.index])));
|
||||
if let Some(ray)=get_push_ray_1(point,c0){
|
||||
if Planar64::ZERO<=c1.relative_dot(ray.direction){
|
||||
return Some((ray,Conts::from_iter([c0])));
|
||||
}
|
||||
}
|
||||
return None;
|
||||
}
|
||||
fn get_best_push_ray_and_indices_3(point:Planar64Vec3,ci0:CI,ci1:CI,ci2:CI)->Option<(Ray,Indices)>{
|
||||
if is_space_enclosed_3(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal){
|
||||
fn get_best_push_ray_and_conts_3<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact)->Option<(Ray,Conts<'a>)>{
|
||||
if is_space_enclosed_3(c0.normal,c1.normal,c2.normal){
|
||||
return None;
|
||||
}
|
||||
if let Some(ray)=get_push_ray_3(point,ci0.contact,ci1.contact,ci2.contact){
|
||||
return Some((ray,Indices::from_iter([ci0.index,ci1.index,ci2.index])));
|
||||
if let Some(ray)=get_push_ray_3(point,c0,c1,c2){
|
||||
return Some((ray,Conts::from_iter([c0,c1,c2])));
|
||||
}
|
||||
if let Some(ray)=get_push_ray_2(point,ci0.contact,ci1.contact){
|
||||
if Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){
|
||||
return Some((ray,Indices::from_iter([ci0.index,ci1.index])));
|
||||
if let Some(ray)=get_push_ray_2(point,c0,c1){
|
||||
if Planar64::ZERO<=c2.relative_dot(ray.direction){
|
||||
return Some((ray,Conts::from_iter([c0,c1])));
|
||||
}
|
||||
}
|
||||
if let Some(ray)=get_push_ray_2(point,ci0.contact,ci2.contact){
|
||||
if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){
|
||||
return Some((ray,Indices::from_iter([ci0.index,ci2.index])));
|
||||
if let Some(ray)=get_push_ray_2(point,c0,c2){
|
||||
if Planar64::ZERO<=c1.relative_dot(ray.direction){
|
||||
return Some((ray,Conts::from_iter([c0,c2])));
|
||||
}
|
||||
}
|
||||
if let Some(ray)=get_push_ray_1(point,ci0.contact){
|
||||
if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction)
|
||||
&&Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){
|
||||
return Some((ray,Indices::from_iter([ci0.index])));
|
||||
if let Some(ray)=get_push_ray_1(point,c0){
|
||||
if Planar64::ZERO<=c1.relative_dot(ray.direction)
|
||||
&&Planar64::ZERO<=c2.relative_dot(ray.direction){
|
||||
return Some((ray,Conts::from_iter([c0])));
|
||||
}
|
||||
}
|
||||
return None;
|
||||
}
|
||||
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(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal,ci3.contact.normal){
|
||||
fn get_best_push_ray_and_conts_4<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact,c3:&'a Contact)->Option<(Ray,Conts<'a>)>{
|
||||
if is_space_enclosed_4(c0.normal,c1.normal,c2.normal,c3.normal){
|
||||
return None;
|
||||
}
|
||||
|
||||
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,ci0,ci1,ci3)?;
|
||||
let (ray023,indices023)=get_best_push_ray_and_indices_3(point,ci0,ci2,ci3)?;
|
||||
let (ray012,conts012)=get_best_push_ray_and_conts_3(point,c0,c1,c2)?;
|
||||
let (ray013,conts013)=get_best_push_ray_and_conts_3(point,c0,c1,c3)?;
|
||||
let (ray023,conts023)=get_best_push_ray_and_conts_3(point,c0,c2,c3)?;
|
||||
|
||||
let err012=ci3.contact.relative_dot(ray012.direction);
|
||||
let err013=ci2.contact.relative_dot(ray013.direction);
|
||||
let err023=ci1.contact.relative_dot(ray023.direction);
|
||||
let err012=c3.relative_dot(ray012.direction);
|
||||
let err013=c2.relative_dot(ray013.direction);
|
||||
let err023=c1.relative_dot(ray023.direction);
|
||||
|
||||
let best_err=err012.max(err013).max(err023);
|
||||
|
||||
if best_err==err012{
|
||||
return Some((ray012,indices012))
|
||||
return Some((ray012,conts012))
|
||||
}else if best_err==err013{
|
||||
return Some((ray013,indices013))
|
||||
return Some((ray013,conts013))
|
||||
}else if best_err==err023{
|
||||
return Some((ray023,indices023))
|
||||
return Some((ray023,conts023))
|
||||
}
|
||||
unreachable!()
|
||||
}
|
||||
|
||||
fn get_best_push_ray_and_indices(
|
||||
fn get_best_push_ray_and_conts<'a>(
|
||||
point:Planar64Vec3,
|
||||
conts:&Vec<Contact>,
|
||||
indices:Indices,
|
||||
)->Option<(Ray,Indices)>{
|
||||
match indices.as_slice(){
|
||||
&[i0,i1,i2,i3]=>get_best_push_ray_and_indices_4(point,CI::new(conts,i0),CI::new(conts,i1),CI::new(conts,i2),CI::new(conts,i3)),
|
||||
&[i0,i1,i2]=>get_best_push_ray_and_indices_3(point,CI::new(conts,i0),CI::new(conts,i1),CI::new(conts,i2)),
|
||||
&[i0,i1]=>get_best_push_ray_and_indices_2(point,CI::new(conts,i0),CI::new(conts,i1)),
|
||||
&[i0]=>get_best_push_ray_and_indices_1(point,CI::new(conts,i0)),
|
||||
&[]=>get_best_push_ray_and_indices_0(point),
|
||||
conts:Conts<'a>,
|
||||
)->Option<(Ray,Conts<'a>)>{
|
||||
match conts.as_slice(){
|
||||
&[c0,c1,c2,c3]=>get_best_push_ray_and_conts_4(point,c0,c1,c2,c3),
|
||||
&[c0,c1,c2]=>get_best_push_ray_and_conts_3(point,c0,c1,c2),
|
||||
&[c0,c1]=>get_best_push_ray_and_conts_2(point,c0,c1),
|
||||
&[c0]=>get_best_push_ray_and_conts_1(point,c0),
|
||||
&[]=>get_best_push_ray_and_conts_0(point),
|
||||
_=>unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
fn get_first_touch(conts:&Vec<Contact>,ray:&Ray,indices:&Indices)->Option<(Planar64,usize)>{
|
||||
conts.iter()
|
||||
.enumerate()
|
||||
.filter(|&(i,contact)|
|
||||
!indices.contains(&i)
|
||||
fn get_first_touch<'a>(contacts:&'a Vec<Contact>,ray:&Ray,conts:&Conts)->Option<(Planar64,&'a Contact)>{
|
||||
contacts.iter()
|
||||
.filter(|&contact|
|
||||
!conts.iter().any(|&c|std::ptr::eq(c,contact))
|
||||
&&contact.relative_dot(ray.direction)< -EPSILON
|
||||
)
|
||||
.map(|(i,contact)|(contact.solve(ray),i))
|
||||
.map(|contact|(contact.solve(ray),contact))
|
||||
.min_by_key(|&(t,_)|t)
|
||||
}
|
||||
|
||||
pub fn push_solve(conts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
|
||||
let (mut ray,mut indices)=get_best_push_ray_and_indices_0(point)?;
|
||||
pub fn push_solve(contacts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
|
||||
let (mut ray,mut conts)=get_best_push_ray_and_conts_0(point)?;
|
||||
loop{
|
||||
let (next_t,next_index)=get_first_touch(conts,&ray,&indices)?;
|
||||
let (next_t,next_cont)=get_first_touch(contacts,&ray,&conts)?;
|
||||
|
||||
if Planar64::ZERO<=next_t{
|
||||
return Some(ray.origin);
|
||||
}
|
||||
|
||||
//edit the indices
|
||||
if indices.len()==4{
|
||||
indices.pop();
|
||||
if conts.len()==4{
|
||||
conts.pop();
|
||||
}
|
||||
indices.push(next_index);
|
||||
conts.push(next_cont);
|
||||
|
||||
let meet_point=ray.extrapolate(next_t);
|
||||
match get_best_push_ray_and_indices(meet_point,conts,indices){
|
||||
Some((new_ray,new_indices))=>(ray,indices)=(new_ray,new_indices),
|
||||
match get_best_push_ray_and_conts(meet_point,conts){
|
||||
Some((new_ray,new_conts))=>(ray,conts)=(new_ray,new_conts),
|
||||
None=>return Some(meet_point),
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user