improve closest_time_to_point accuracy

lib/src/bvh.rs

@@ -2,12 +2,16 @@ 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;
+use strafesnet_common::integer::{Fixed,Planar64};
 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(Range<usize>);
+struct EventSlice{
+	slice:Range<usize>,
+	inclusive:bool,
+}
 
 pub struct Bvh{
 	bvh:strafesnet_common::bvh::BvhNode<EventSlice>,
@@ -40,19 +44,23 @@ impl Bvh{
 			// 0123456789
 			// split into groups of MAX_SLICE_LEN=4
 			// [0123][4567][89]
-			let mut push_slice=|slice:Range<usize>|{
+			let mut push_slice=|slice:Range<usize>,inclusive:bool|{
 				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());
 				}
-				bvh_nodes.push((EventSlice(slice),aabb));
+				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));
 			};
 			// push fixed-size groups
 			for i in 0..count-1{
-				push_slice((last_index+i*slice_len)..(last_index+(i+1)*slice_len));
+				push_slice((last_index+i*slice_len)..(last_index+(i+1)*slice_len),true);
 			}
 			// push last group which may be shorter
-			push_slice((last_index+(count-1)*slice_len)..index);
+			push_slice((last_index+(count-1)*slice_len)..index,false);
 			last_index=index;
 		};
 		// find discontinuities (teleports) and avoid forming a bvh node across them
@@ -66,31 +74,89 @@ 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(&self,bot:&CompleteBot,point:glam::Vec3)->Option<PhysicsTime>{
+	pub fn closest_time_to_point<'a>(&self,bot:&'a CompleteBot,point:glam::Vec3)->Option<PhysicsTime>{
 		let point=point+bot.world_offset();
 		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=|EventSlice(slice):&EventSlice|{
+		let intersect_leaf=|event_slice:&EventSlice|{
 			// calculate the distance to the leaf contents
-			output_events[slice.start..slice.end].iter().map(|event|{
+			let mut best_distance=output_events[event_slice.slice.start..event_slice.slice.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()
+			}).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)
 		};
 		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 (_,EventSlice(slice))=self.bvh.traverse(start_point,Fixed::ZERO,Fixed::MAX,intersect_leaf,intersect_aabb)?;
+		let (_,event_slice)=self.bvh.traverse(start_point,Fixed::ZERO,Fixed::MAX,intersect_leaf,intersect_aabb)?;
-		let closest_event=output_events[slice.start..slice.end].iter().min_by_key(|&event|{
+
+		// 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 p=event.event.position;
 			let p=vec3::try_from_f32_array([p.x,p.y,p.z]).unwrap();
-			(start_point-p).length_squared()
+			(event.time,(start_point-p).length_squared())
-		})?;
+		}).min_by_key(|&(_,distance)|distance)?;
-		// TODO: project start_point onto the edges connected to the closest_event and return the true time
+		let mut best_time=crate::time::from_float(best_time).unwrap();
-		crate::time::from_float(closest_event.time).ok()
+		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)
 	}
 }