|
|
|
|
@@ -2,16 +2,12 @@ use core::ops::Range;
|
|
|
|
|
use strafesnet_common::aabb::Aabb;
|
|
|
|
|
use strafesnet_common::bvh::generate_bvh;
|
|
|
|
|
use strafesnet_common::integer::vec3;
|
|
|
|
|
use strafesnet_common::integer::{Fixed,Planar64};
|
|
|
|
|
use strafesnet_common::integer::Fixed;
|
|
|
|
|
use strafesnet_common::physics::Time as PhysicsTime;
|
|
|
|
|
use crate::bot::CompleteBot;
|
|
|
|
|
use strafesnet_roblox_bot_file::v0;
|
|
|
|
|
|
|
|
|
|
const MAX_SLICE_LEN:usize=16;
|
|
|
|
|
struct EventSlice{
|
|
|
|
|
slice:Range<usize>,
|
|
|
|
|
inclusive:bool,
|
|
|
|
|
}
|
|
|
|
|
struct EventSlice(Range<usize>);
|
|
|
|
|
|
|
|
|
|
pub struct Bvh{
|
|
|
|
|
bvh:strafesnet_common::bvh::BvhNode<EventSlice>,
|
|
|
|
|
@@ -20,7 +16,7 @@ pub struct Bvh{
|
|
|
|
|
impl Bvh{
|
|
|
|
|
pub fn new(bot:&CompleteBot)->Self{
|
|
|
|
|
let output_events=&bot.timelines().output_events;
|
|
|
|
|
// iterator over the event timeline and capture slices of contiguous output events.
|
|
|
|
|
// iterator over the event timeline and capture slizes of contiguous output events.
|
|
|
|
|
// create an Aabb for each slice and then generate a BVH.
|
|
|
|
|
let mut bvh_nodes=Vec::new();
|
|
|
|
|
let it=output_events
|
|
|
|
|
@@ -29,9 +25,9 @@ impl Bvh{
|
|
|
|
|
// find discontinuities
|
|
|
|
|
.filter(|&(_,[event0,event1])|
|
|
|
|
|
event0.time==event1.time&&!(
|
|
|
|
|
event0.event.position.x==event1.event.position.x
|
|
|
|
|
&&event0.event.position.y==event1.event.position.y
|
|
|
|
|
&&event0.event.position.z==event1.event.position.z
|
|
|
|
|
event0.event.position.x==event0.event.position.x
|
|
|
|
|
&&event0.event.position.y==event0.event.position.y
|
|
|
|
|
&&event0.event.position.z==event0.event.position.z
|
|
|
|
|
)
|
|
|
|
|
);
|
|
|
|
|
|
|
|
|
|
@@ -39,31 +35,33 @@ impl Bvh{
|
|
|
|
|
let mut push_slices=|index:usize|{
|
|
|
|
|
let len=index-last_index;
|
|
|
|
|
let count=len.div_ceil(MAX_SLICE_LEN);
|
|
|
|
|
let slice_len=MAX_SLICE_LEN;
|
|
|
|
|
let slice_len=len/count;
|
|
|
|
|
bvh_nodes.reserve(count);
|
|
|
|
|
// 0123456789
|
|
|
|
|
// split into groups of MAX_SLICE_LEN=4
|
|
|
|
|
// last_index=0
|
|
|
|
|
// split_index=9
|
|
|
|
|
// index=10
|
|
|
|
|
// len=10
|
|
|
|
|
// count=3
|
|
|
|
|
// node_len=4
|
|
|
|
|
// split into groups of 4
|
|
|
|
|
// [0123][4567][89]
|
|
|
|
|
let mut push_slice=|slice:Range<usize>,inclusive:bool|{
|
|
|
|
|
let mut push_slice=|slice:Range<usize>|{
|
|
|
|
|
let mut aabb=Aabb::default();
|
|
|
|
|
for event in &output_events[slice.start..slice.end]{
|
|
|
|
|
aabb.grow(vec3::try_from_f32_array([event.event.position.x,event.event.position.y,event.event.position.z]).unwrap());
|
|
|
|
|
}
|
|
|
|
|
if inclusive{
|
|
|
|
|
let event=&output_events[slice.end];
|
|
|
|
|
aabb.grow(vec3::try_from_f32_array([event.event.position.x,event.event.position.y,event.event.position.z]).unwrap());
|
|
|
|
|
}
|
|
|
|
|
bvh_nodes.push((EventSlice{slice,inclusive},aabb));
|
|
|
|
|
bvh_nodes.push((EventSlice(slice),aabb));
|
|
|
|
|
};
|
|
|
|
|
// push fixed-size groups
|
|
|
|
|
for i in 0..count-1{
|
|
|
|
|
push_slice((last_index+i*slice_len)..(last_index+(i+1)*slice_len),true);
|
|
|
|
|
push_slice((last_index+i*slice_len)..(last_index+(i+1)*slice_len));
|
|
|
|
|
}
|
|
|
|
|
// push last group which may be shorter
|
|
|
|
|
push_slice((last_index+(count-1)*slice_len)..index,false);
|
|
|
|
|
push_slice((last_index+(count-1)*slice_len)..index);
|
|
|
|
|
last_index=index;
|
|
|
|
|
};
|
|
|
|
|
// find discontinuities (teleports) and avoid forming a bvh node across them
|
|
|
|
|
// find discontinuities (teleports) and avoid forming a nvh node across them
|
|
|
|
|
for (split_index,_) in it{
|
|
|
|
|
// we want to use the index of event1
|
|
|
|
|
push_slices(split_index+1);
|
|
|
|
|
@@ -74,89 +72,30 @@ impl Bvh{
|
|
|
|
|
Self{bvh}
|
|
|
|
|
}
|
|
|
|
|
/// Find the exact timestamp on the bot timeline that is closest to the given point.
|
|
|
|
|
pub fn closest_time_to_point<'a>(&self,bot:&'a CompleteBot,point:glam::Vec3)->Option<PhysicsTime>{
|
|
|
|
|
let point=point+bot.world_offset();
|
|
|
|
|
pub fn closest_time_to_point(&self,bot:&CompleteBot,point:glam::Vec3)->Option<PhysicsTime>{
|
|
|
|
|
let start_point=vec3::try_from_f32_array(point.to_array()).unwrap();
|
|
|
|
|
let output_events=&bot.timelines().output_events;
|
|
|
|
|
// grow a sphere starting at start_point until we find the closest point on the bot output events
|
|
|
|
|
let intersect_leaf=|event_slice:&EventSlice|{
|
|
|
|
|
let intersect_leaf=|leaf:&EventSlice|{
|
|
|
|
|
// calculate the distance to the leaf contents
|
|
|
|
|
let mut best_distance=output_events[event_slice.slice.start..event_slice.slice.end].iter().map(|event|{
|
|
|
|
|
output_events[leaf.0.start..leaf.0.end].iter().map(|event|{
|
|
|
|
|
let p=event.event.position;
|
|
|
|
|
let p=vec3::try_from_f32_array([p.x,p.y,p.z]).unwrap();
|
|
|
|
|
(start_point-p).length_squared()
|
|
|
|
|
}).min()?;
|
|
|
|
|
let mut prev_event=&output_events[event_slice.slice.start];
|
|
|
|
|
let mut f=|event:&'a v0::Timed<v0::OutputEvent>|{
|
|
|
|
|
let p0=vec3::try_from_f32_array([prev_event.event.position.x,prev_event.event.position.y,prev_event.event.position.z]).unwrap();
|
|
|
|
|
let p1=vec3::try_from_f32_array([event.event.position.x,event.event.position.y,event.event.position.z]).unwrap();
|
|
|
|
|
let d=p1-p0;
|
|
|
|
|
let d0=p0.dot(d);
|
|
|
|
|
let d1=p1.dot(d);
|
|
|
|
|
let sp_d=start_point.dot(d);
|
|
|
|
|
// must be on the segment
|
|
|
|
|
if d0<sp_d&&sp_d<d1{
|
|
|
|
|
let t0=d1-sp_d;
|
|
|
|
|
let t1=sp_d-d0;
|
|
|
|
|
let dt=d1-d0;
|
|
|
|
|
let distance=(((p0*t0+p1*t1)/dt).divide().wrap_1()-start_point).length_squared();
|
|
|
|
|
if distance<best_distance{
|
|
|
|
|
best_distance=distance;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
prev_event=event;
|
|
|
|
|
};
|
|
|
|
|
for event in &output_events[event_slice.slice.start+1..event_slice.slice.end]{
|
|
|
|
|
f(event);
|
|
|
|
|
}
|
|
|
|
|
if event_slice.inclusive{
|
|
|
|
|
f(&output_events[event_slice.slice.end]);
|
|
|
|
|
}
|
|
|
|
|
Some(best_distance)
|
|
|
|
|
}).min()
|
|
|
|
|
};
|
|
|
|
|
let intersect_aabb=|aabb:&Aabb|{
|
|
|
|
|
// calculate the distance to the aabb
|
|
|
|
|
let clamped_point=start_point.min(aabb.max()).max(aabb.min());
|
|
|
|
|
Some((start_point-clamped_point).length_squared())
|
|
|
|
|
};
|
|
|
|
|
let (_,event_slice)=self.bvh.traverse(start_point,Fixed::ZERO,Fixed::MAX,intersect_leaf,intersect_aabb)?;
|
|
|
|
|
|
|
|
|
|
// find time at the closest point
|
|
|
|
|
let (best_time,mut best_distance)=output_events[event_slice.slice.start..event_slice.slice.end].iter().map(|event|{
|
|
|
|
|
let (_,leaf)=self.bvh.traverse(start_point,Fixed::ZERO,Fixed::MAX,intersect_leaf,intersect_aabb)?;
|
|
|
|
|
let closest_event=output_events[leaf.0.start..leaf.0.end].iter().min_by_key(|&event|{
|
|
|
|
|
let p=event.event.position;
|
|
|
|
|
let p=vec3::try_from_f32_array([p.x,p.y,p.z]).unwrap();
|
|
|
|
|
(event.time,(start_point-p).length_squared())
|
|
|
|
|
}).min_by_key(|&(_,distance)|distance)?;
|
|
|
|
|
let mut best_time=crate::time::from_float(best_time).unwrap();
|
|
|
|
|
let mut prev_event=&output_events[event_slice.slice.start];
|
|
|
|
|
let mut f=|event:&'a v0::Timed<v0::OutputEvent>|{
|
|
|
|
|
let p0=vec3::try_from_f32_array([prev_event.event.position.x,prev_event.event.position.y,prev_event.event.position.z]).unwrap();
|
|
|
|
|
let p1=vec3::try_from_f32_array([event.event.position.x,event.event.position.y,event.event.position.z]).unwrap();
|
|
|
|
|
let d=p1-p0;
|
|
|
|
|
let d0=p0.dot(d);
|
|
|
|
|
let d1=p1.dot(d);
|
|
|
|
|
let sp_d=start_point.dot(d);
|
|
|
|
|
// must be on the segment
|
|
|
|
|
if d0<sp_d&&sp_d<d1{
|
|
|
|
|
let t0=d1-sp_d;
|
|
|
|
|
let t1=sp_d-d0;
|
|
|
|
|
let dt=d1-d0;
|
|
|
|
|
let distance=(((p0*t0+p1*t1)/dt).divide().wrap_1()-start_point).length_squared();
|
|
|
|
|
if distance<best_distance{
|
|
|
|
|
best_distance=distance;
|
|
|
|
|
let p0:Planar64=prev_event.time.try_into().unwrap();
|
|
|
|
|
let p1:Planar64=event.time.try_into().unwrap();
|
|
|
|
|
best_time=((p0*t0+p1*t1)/dt).into();
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
prev_event=event;
|
|
|
|
|
};
|
|
|
|
|
for event in &output_events[event_slice.slice.start+1..event_slice.slice.end]{
|
|
|
|
|
f(event);
|
|
|
|
|
}
|
|
|
|
|
if event_slice.inclusive{
|
|
|
|
|
f(&output_events[event_slice.slice.end]);
|
|
|
|
|
}
|
|
|
|
|
Some(best_time)
|
|
|
|
|
(start_point-p).length_squared()
|
|
|
|
|
})?;
|
|
|
|
|
// TODO: project start_point onto the edges connected to the closest_event and return the true time
|
|
|
|
|
crate::time::from_float(closest_event.time).ok()
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|