strafe-client-jed/strafe-client/src/body.rs

use strafesnet_common::aabb;
use strafesnet_common::integer::{self,vec3,Time,Planar64,Planar64Vec3};
#[derive(Clone,Copy,Debug,Hash)]
pub struct Body<T>{
	pub position:Planar64Vec3,//I64 where 2^32 = 1 u
	pub velocity:Planar64Vec3,//I64 where 2^32 = 1 u/s
	pub acceleration:Planar64Vec3,//I64 where 2^32 = 1 u/s/s
	pub time:Time<T>,//nanoseconds x xxxxD!
}
impl<T> std::ops::Neg for Body<T>{
	type Output=Self;
	fn neg(self)->Self::Output{
		Self{
			position:self.position,
			velocity:-self.velocity,
			acceleration:self.acceleration,
			time:-self.time,
		}
	}
}

impl<T> Body<T>
	where Time<T>:Copy,
{
	pub const ZERO:Self=Self::new(vec3::ZERO,vec3::ZERO,vec3::ZERO,Time::ZERO);
	pub const fn new(position:Planar64Vec3,velocity:Planar64Vec3,acceleration:Planar64Vec3,time:Time<T>)->Self{
		Self{
			position,
			velocity,
			acceleration,
			time,
		}
	}
	pub fn extrapolated_position(&self,time:Time<T>)->Planar64Vec3{
		let dt=time-self.time;
		self.position
		+(self.velocity*dt).map(|elem|elem.divide().fix_1())
		+self.acceleration.map(|elem|(dt*dt*elem/2).divide().fix_1())
	}
	pub fn extrapolated_velocity(&self,time:Time<T>)->Planar64Vec3{
		let dt=time-self.time;
		self.velocity+(self.acceleration*dt).map(|elem|elem.divide().fix_1())
	}
	pub fn advance_time(&mut self,time:Time<T>){
		self.position=self.extrapolated_position(time);
		self.velocity=self.extrapolated_velocity(time);
		self.time=time;
	}
	pub fn extrapolated_position_ratio_dt<Num,Den,N1,D1,N2,N3,D2,N4,T1>(&self,dt:integer::Ratio<Num,Den>)->Planar64Vec3
		where
			// Why?
			// All of this can be removed with const generics because the type can be specified as
			// Ratio<Fixed<N,NF>,Fixed<D,DF>>
			// which is known to implement all the necessary traits
			Num:Copy,
			Den:Copy+core::ops::Mul<i64,Output=D1>,
			D1:Copy,
			Num:core::ops::Mul<Planar64,Output=N1>,
			Planar64:core::ops::Mul<D1,Output=N2>,
			N1:core::ops::Add<N2,Output=N3>,
			Num:core::ops::Mul<N3,Output=N4>,
			Den:core::ops::Mul<D1,Output=D2>,
			D2:Copy,
			Planar64:core::ops::Mul<D2,Output=N4>,
			N4:integer::Divide<D2,Output=T1>,
			T1:integer::Fix<Planar64>,
	{
		// a*dt^2/2 + v*dt + p
		// (a*dt/2+v)*dt+p
		(self.acceleration.map(|elem|dt*elem/2)+self.velocity).map(|elem|dt.mul_ratio(elem))
		.map(|elem|elem.divide().fix())+self.position
	}
	pub fn extrapolated_velocity_ratio_dt<Num,Den,N1,T1>(&self,dt:integer::Ratio<Num,Den>)->Planar64Vec3
		where
			Num:Copy,
			Den:Copy,
			Num:core::ops::Mul<Planar64,Output=N1>,
			Planar64:core::ops::Mul<Den,Output=N1>,
			N1:integer::Divide<Den,Output=T1>,
			T1:integer::Fix<Planar64>,
	{
		// a*dt + v
		self.acceleration.map(|elem|(dt*elem).divide().fix())+self.velocity
	}
	pub fn advance_time_ratio_dt(&mut self,dt:crate::model_physics::GigaTime){
		self.position=self.extrapolated_position_ratio_dt(dt);
		self.velocity=self.extrapolated_velocity_ratio_dt(dt);
		self.time+=dt.into();
	}
	pub fn infinity_dir(&self)->Option<Planar64Vec3>{
		if self.velocity==vec3::ZERO{
			if self.acceleration==vec3::ZERO{
				None
			}else{
				Some(self.acceleration)
			}
		}else{
			Some(self.velocity)
		}
	}
	pub fn grow_aabb(&self,aabb:&mut aabb::Aabb,t0:Time<T>,t1:Time<T>){
		aabb.grow(self.extrapolated_position(t0));
		aabb.grow(self.extrapolated_position(t1));
		//v+a*t==0
		//goober code
		if !self.acceleration.x.is_zero(){
			let t=-self.velocity.x/self.acceleration.x;
			if t0.to_ratio().lt_ratio(t)&&t.lt_ratio(t1.to_ratio()){
				aabb.grow(self.extrapolated_position_ratio_dt(t));
			}
		}
		if !self.acceleration.y.is_zero(){
			let t=-self.velocity.y/self.acceleration.y;
			if t0.to_ratio().lt_ratio(t)&&t.lt_ratio(t1.to_ratio()){
				aabb.grow(self.extrapolated_position_ratio_dt(t));
			}
		}
		if !self.acceleration.z.is_zero(){
			let t=-self.velocity.z/self.acceleration.z;
			if t0.to_ratio().lt_ratio(t)&&t.lt_ratio(t1.to_ratio()){
				aabb.grow(self.extrapolated_position_ratio_dt(t));
			}
		}
	}

}
impl<T> std::fmt::Display for Body<T>{
	fn fmt(&self,f:&mut std::fmt::Formatter<'_>)->std::fmt::Result{
		write!(f,"p({}) v({}) a({}) t({})",self.position,self.velocity,self.acceleration,self.time)
	}
}

pub struct VirtualBody<'a,T>{
	body0:&'a Body<T>,
	body1:&'a Body<T>,
}
impl<T> VirtualBody<'_,T>
	where Time<T>:Copy,
{
	pub const fn relative<'a>(body0:&'a Body<T>,body1:&'a Body<T>)->VirtualBody<'a,T>{
		//(p0,v0,a0,t0)
		//(p1,v1,a1,t1)
		VirtualBody{
			body0,
			body1,
		}
	}
	pub fn extrapolated_position(&self,time:Time<T>)->Planar64Vec3{
		self.body1.extrapolated_position(time)-self.body0.extrapolated_position(time)
	}
	pub fn extrapolated_velocity(&self,time:Time<T>)->Planar64Vec3{
		self.body1.extrapolated_velocity(time)-self.body0.extrapolated_velocity(time)
	}
	pub fn acceleration(&self)->Planar64Vec3{
		self.body1.acceleration-self.body0.acceleration
	}
	pub fn body(&self,time:Time<T>)->Body<T>{
		Body::new(self.extrapolated_position(time),self.extrapolated_velocity(time),self.acceleration(),time)
	}
}