MeshQuery trait FEV associated types

This commit is contained in:
Quaternions 2025-01-08 20:51:13 -08:00
parent ca8035cdfc
commit 6066e82fd2
2 changed files with 86 additions and 67 deletions

View File

@ -1,23 +1,34 @@
use crate::model_physics::{GigaTime,FEV,MeshQuery,DirectedEdge,MinkowskiMesh,MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert}; use crate::model_physics::{GigaTime,FEV,MeshQuery,DirectedEdge};
use strafesnet_common::integer::{Fixed,Ratio}; use strafesnet_common::integer::{Fixed,Ratio,vec3::Vector3};
use crate::physics::{Time,Body}; use crate::physics::{Time,Body};
#[derive(Debug)] enum Transition<M:MeshQuery>{
enum Transition<F,E:DirectedEdge,V>{
Miss, Miss,
Next(FEV<F,E,V>,GigaTime), Next(FEV<M>,GigaTime),
Hit(F,GigaTime), Hit(M::Face,GigaTime),
} }
type MinkowskiFEV=FEV<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>; pub enum CrawlResult<M:MeshQuery>{
type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>; Miss(FEV<M>),
Hit(M::Face,GigaTime),
}
fn next_transition(fev:&MinkowskiFEV,body_time:GigaTime,mesh:&MinkowskiMesh,body:&Body,mut best_time:GigaTime)->MinkowskiTransition{ impl<F:Copy,M:MeshQuery<Normal=Vector3<F>,Offset=Fixed<4,128>>> FEV<M>
where
// This is hardcoded for MinkowskiMesh lol
M::Face:Copy,
M::Edge:Copy,
M::Vert:Copy,
F:core::ops::Mul<Fixed<1,32>,Output=Fixed<4,128>>,
<F as core::ops::Mul<Fixed<1,32>>>::Output:core::iter::Sum,
<M as MeshQuery>::Offset:core::ops::Sub<<F as std::ops::Mul<Fixed<1,32>>>::Output>,
{
fn next_transition(&self,body_time:GigaTime,mesh:&M,body:&Body,mut best_time:GigaTime)->Transition<M>{
//conflicting derivative means it crosses in the wrong direction. //conflicting derivative means it crosses in the wrong direction.
//if the transition time is equal to an already tested transition, do not replace the current best. //if the transition time is equal to an already tested transition, do not replace the current best.
let mut best_transition=MinkowskiTransition::Miss; let mut best_transition=Transition::Miss;
match fev{ match self{
&MinkowskiFEV::Face(face_id)=>{ &FEV::Face(face_id)=>{
//test own face collision time, ignoring roots with zero or conflicting derivative //test own face collision time, ignoring roots with zero or conflicting derivative
//n=face.normal d=face.dot //n=face.normal d=face.dot
//n.a t^2+n.v t+n.p-d==0 //n.a t^2+n.v t+n.p-d==0
@ -27,7 +38,7 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
for dt in Fixed::<4,128>::zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ for dt in Fixed::<4,128>::zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
best_time=dt; best_time=dt;
best_transition=MinkowskiTransition::Hit(face_id,dt); best_transition=Transition::Hit(face_id,dt);
break; break;
} }
} }
@ -41,14 +52,14 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
for dt in Fixed::<4,128>::zeroes2(n.dot(body.position*2-(mesh.vert(verts[0])+mesh.vert(verts[1]))).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){ for dt in Fixed::<4,128>::zeroes2(n.dot(body.position*2-(mesh.vert(verts[0])+mesh.vert(verts[1]))).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){
if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
best_time=dt; best_time=dt;
best_transition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt); best_transition=Transition::Next(FEV::Edge(directed_edge_id.as_undirected()),dt);
break; break;
} }
} }
} }
//if none: //if none:
}, },
&MinkowskiFEV::Edge(edge_id)=>{ &FEV::Edge(edge_id)=>{
//test each face collision time, ignoring roots with zero or conflicting derivative //test each face collision time, ignoring roots with zero or conflicting derivative
let edge_n=mesh.edge_n(edge_id); let edge_n=mesh.edge_n(edge_id);
let edge_verts=mesh.edge_verts(edge_id); let edge_verts=mesh.edge_verts(edge_id);
@ -61,7 +72,7 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
for dt in Fixed::<4,128>::zeroes2(n.dot(delta_pos).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){ for dt in Fixed::<4,128>::zeroes2(n.dot(delta_pos).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){
if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
best_time=dt; best_time=dt;
best_transition=MinkowskiTransition::Next(MinkowskiFEV::Face(edge_face_id),dt); best_transition=Transition::Next(FEV::Face(edge_face_id),dt);
break; break;
} }
} }
@ -74,14 +85,14 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4()); let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4());
best_time=dt; best_time=dt;
best_transition=MinkowskiTransition::Next(MinkowskiFEV::Vert(vert_id),dt); best_transition=Transition::Next(FEV::Vert(vert_id),dt);
break; break;
} }
} }
} }
//if none: //if none:
}, },
&MinkowskiFEV::Vert(vert_id)=>{ &FEV::Vert(vert_id)=>{
//test each edge collision time, ignoring roots with zero or conflicting derivative //test each edge collision time, ignoring roots with zero or conflicting derivative
for &directed_edge_id in mesh.vert_edges(vert_id).iter(){ for &directed_edge_id in mesh.vert_edges(vert_id).iter(){
//edge is directed away from vertex, but we want the dot product to turn out negative //edge is directed away from vertex, but we want the dot product to turn out negative
@ -90,7 +101,7 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4()); let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4());
best_time=dt; best_time=dt;
best_transition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt); best_transition=Transition::Next(FEV::Edge(directed_edge_id.as_undirected()),dt);
break; break;
} }
} }
@ -100,28 +111,24 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
} }
best_transition best_transition
} }
pub enum CrawlResult<F,E:DirectedEdge,V>{ pub fn crawl_fev(mut self,mesh:&M,relative_body:&Body,start_time:Time,time_limit:Time)->CrawlResult<M>{
Miss(FEV<F,E,V>), let mut body_time={
Hit(F,GigaTime), let r=(start_time-relative_body.time).to_ratio();
} Ratio::new(r.num.fix_4(),r.den.fix_4())
type MinkowskiCrawlResult=CrawlResult<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>; };
pub fn crawl_fev(mut fev:MinkowskiFEV,mesh:&MinkowskiMesh,relative_body:&Body,start_time:Time,time_limit:Time)->MinkowskiCrawlResult{ let time_limit={
let mut body_time={ let r=(time_limit-relative_body.time).to_ratio();
let r=(start_time-relative_body.time).to_ratio(); Ratio::new(r.num.fix_4(),r.den.fix_4())
Ratio::new(r.num.fix_4(),r.den.fix_4()) };
}; for _ in 0..20{
let time_limit={ match self.next_transition(body_time,mesh,relative_body,time_limit){
let r=(time_limit-relative_body.time).to_ratio(); Transition::Miss=>return CrawlResult::Miss(self),
Ratio::new(r.num.fix_4(),r.den.fix_4()) Transition::Next(next_fev,next_time)=>(self,body_time)=(next_fev,next_time),
}; Transition::Hit(face,time)=>return CrawlResult::Hit(face,time),
for _ in 0..20{ }
match next_transition(&fev,body_time,mesh,relative_body,time_limit){
Transition::Miss=>return CrawlResult::Miss(fev),
Transition::Next(next_fev,next_time)=>(fev,body_time)=(next_fev,next_time),
Transition::Hit(face,time)=>return CrawlResult::Hit(face,time),
} }
//TODO: fix all bugs
//println!("Too many iterations! Using default behaviour instead of crashing...");
CrawlResult::Miss(self)
} }
//TODO: fix all bugs
//println!("Too many iterations! Using default behaviour instead of crashing...");
CrawlResult::Miss(fev)
} }

View File

@ -54,10 +54,10 @@ impl DirectedEdge for SubmeshDirectedEdgeId{
//Vertex <-> Edge <-> Face -> Collide //Vertex <-> Edge <-> Face -> Collide
#[derive(Debug)] #[derive(Debug)]
pub enum FEV<F,E:DirectedEdge,V>{ pub enum FEV<M:MeshQuery>{
Face(F), Face(M::Face),
Edge(E::UndirectedEdge), Edge(<M::Edge as DirectedEdge>::UndirectedEdge),
Vert(V), Vert(M::Vert),
} }
//use Unit32 #[repr(C)] for map files //use Unit32 #[repr(C)] for map files
@ -67,25 +67,28 @@ struct Face{
dot:Planar64, dot:Planar64,
} }
struct Vert(Planar64Vec3); struct Vert(Planar64Vec3);
pub trait MeshQuery<FACE:Clone,EDGE:Clone+DirectedEdge,VERT:Clone>{ pub trait MeshQuery{
type Face:Clone;
type Edge:Clone+DirectedEdge;
type Vert:Clone;
// Vertex must be Planar64Vec3 because it represents an actual position // Vertex must be Planar64Vec3 because it represents an actual position
type Normal; type Normal;
type Offset; type Offset;
fn edge_n(&self,edge_id:EDGE::UndirectedEdge)->Planar64Vec3{ fn edge_n(&self,edge_id:<Self::Edge as DirectedEdge>::UndirectedEdge)->Planar64Vec3{
let verts=self.edge_verts(edge_id); let verts=self.edge_verts(edge_id);
self.vert(verts[1].clone())-self.vert(verts[0].clone()) self.vert(verts[1].clone())-self.vert(verts[0].clone())
} }
fn directed_edge_n(&self,directed_edge_id:EDGE)->Planar64Vec3{ fn directed_edge_n(&self,directed_edge_id:Self::Edge)->Planar64Vec3{
let verts=self.edge_verts(directed_edge_id.as_undirected()); let verts=self.edge_verts(directed_edge_id.as_undirected());
(self.vert(verts[1].clone())-self.vert(verts[0].clone()))*((directed_edge_id.parity() as i64)*2-1) (self.vert(verts[1].clone())-self.vert(verts[0].clone()))*((directed_edge_id.parity() as i64)*2-1)
} }
fn vert(&self,vert_id:VERT)->Planar64Vec3; fn vert(&self,vert_id:Self::Vert)->Planar64Vec3;
fn face_nd(&self,face_id:FACE)->(Self::Normal,Self::Offset); fn face_nd(&self,face_id:Self::Face)->(Self::Normal,Self::Offset);
fn face_edges(&self,face_id:FACE)->Cow<Vec<EDGE>>; fn face_edges(&self,face_id:Self::Face)->Cow<Vec<Self::Edge>>;
fn edge_faces(&self,edge_id:EDGE::UndirectedEdge)->Cow<[FACE;2]>; fn edge_faces(&self,edge_id:<Self::Edge as DirectedEdge>::UndirectedEdge)->Cow<[Self::Face;2]>;
fn edge_verts(&self,edge_id:EDGE::UndirectedEdge)->Cow<[VERT;2]>; fn edge_verts(&self,edge_id:<Self::Edge as DirectedEdge>::UndirectedEdge)->Cow<[Self::Vert;2]>;
fn vert_edges(&self,vert_id:VERT)->Cow<Vec<EDGE>>; fn vert_edges(&self,vert_id:Self::Vert)->Cow<Vec<Self::Edge>>;
fn vert_faces(&self,vert_id:VERT)->Cow<Vec<FACE>>; fn vert_faces(&self,vert_id:Self::Vert)->Cow<Vec<Self::Face>>;
} }
struct FaceRefs{ struct FaceRefs{
edges:Vec<SubmeshDirectedEdgeId>, edges:Vec<SubmeshDirectedEdgeId>,
@ -424,7 +427,10 @@ pub struct PhysicsMeshView<'a>{
data:&'a PhysicsMeshData, data:&'a PhysicsMeshData,
topology:&'a PhysicsMeshTopology, topology:&'a PhysicsMeshTopology,
} }
impl MeshQuery<SubmeshFaceId,SubmeshDirectedEdgeId,SubmeshVertId> for PhysicsMeshView<'_>{ impl MeshQuery for PhysicsMeshView<'_>{
type Face=SubmeshFaceId;
type Edge=SubmeshDirectedEdgeId;
type Vert=SubmeshVertId;
type Normal=Planar64Vec3; type Normal=Planar64Vec3;
type Offset=Planar64; type Offset=Planar64;
fn face_nd(&self,face_id:SubmeshFaceId)->(Planar64Vec3,Planar64){ fn face_nd(&self,face_id:SubmeshFaceId)->(Planar64Vec3,Planar64){
@ -498,7 +504,10 @@ impl TransformedMesh<'_>{
) )
} }
} }
impl MeshQuery<SubmeshFaceId,SubmeshDirectedEdgeId,SubmeshVertId> for TransformedMesh<'_>{ impl MeshQuery for TransformedMesh<'_>{
type Face=SubmeshFaceId;
type Edge=SubmeshDirectedEdgeId;
type Vert=SubmeshVertId;
type Normal=Vector3<Fixed<3,96>>; type Normal=Vector3<Fixed<3,96>>;
type Offset=Fixed<4,128>; type Offset=Fixed<4,128>;
fn face_nd(&self,face_id:SubmeshFaceId)->(Self::Normal,Self::Offset){ fn face_nd(&self,face_id:SubmeshFaceId)->(Self::Normal,Self::Offset){
@ -672,13 +681,13 @@ impl MinkowskiMesh<'_>{
} }
} }
/// This function drops a vertex down to an edge or a face if the path from infinity did not cross any vertex-edge boundaries but the point is supposed to have already crossed a boundary down from a vertex /// This function drops a vertex down to an edge or a face if the path from infinity did not cross any vertex-edge boundaries but the point is supposed to have already crossed a boundary down from a vertex
fn infinity_fev(&self,infinity_dir:Planar64Vec3,point:Planar64Vec3)->FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>{ fn infinity_fev(&self,infinity_dir:Planar64Vec3,point:Planar64Vec3)->FEV::<MinkowskiMesh>{
//start on any vertex //start on any vertex
//cross uncrossable vertex-edge boundaries until you find the closest vertex or edge //cross uncrossable vertex-edge boundaries until you find the closest vertex or edge
//cross edge-face boundary if it's uncrossable //cross edge-face boundary if it's uncrossable
match self.crawl_boundaries(self.farthest_vert(infinity_dir),infinity_dir,point){ match self.crawl_boundaries(self.farthest_vert(infinity_dir),infinity_dir,point){
//if a vert is returned, it is the closest point to the infinity point //if a vert is returned, it is the closest point to the infinity point
EV::Vert(vert_id)=>FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>::Vert(vert_id), EV::Vert(vert_id)=>FEV::Vert(vert_id),
EV::Edge(edge_id)=>{ EV::Edge(edge_id)=>{
//cross to face if the boundary is not crossable and we are on the wrong side //cross to face if the boundary is not crossable and we are on the wrong side
let edge_n=self.edge_n(edge_id); let edge_n=self.edge_n(edge_id);
@ -696,14 +705,14 @@ impl MinkowskiMesh<'_>{
//infinity_dir can always be treated as a velocity //infinity_dir can always be treated as a velocity
if !boundary_d.is_positive()&&boundary_n.dot(infinity_dir).is_zero(){ if !boundary_d.is_positive()&&boundary_n.dot(infinity_dir).is_zero(){
//both faces cannot pass this condition, return early if one does. //both faces cannot pass this condition, return early if one does.
return FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>::Face(face_id); return FEV::Face(face_id);
} }
} }
FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>::Edge(edge_id) FEV::Edge(edge_id)
}, },
} }
} }
fn closest_fev_not_inside(&self,mut infinity_body:Body)->Option<FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>>{ fn closest_fev_not_inside(&self,mut infinity_body:Body)->Option<FEV<MinkowskiMesh>>{
infinity_body.infinity_dir().map_or(None,|dir|{ infinity_body.infinity_dir().map_or(None,|dir|{
let infinity_fev=self.infinity_fev(-dir,infinity_body.position); let infinity_fev=self.infinity_fev(-dir,infinity_body.position);
//a line is simpler to solve than a parabola //a line is simpler to solve than a parabola
@ -711,7 +720,7 @@ impl MinkowskiMesh<'_>{
infinity_body.acceleration=vec3::ZERO; infinity_body.acceleration=vec3::ZERO;
//crawl in from negative infinity along a tangent line to get the closest fev //crawl in from negative infinity along a tangent line to get the closest fev
// TODO: change crawl_fev args to delta time? Optional values? // TODO: change crawl_fev args to delta time? Optional values?
match crate::face_crawler::crawl_fev(infinity_fev,self,&infinity_body,Time::MIN/4,infinity_body.time){ match infinity_fev.crawl_fev(self,&infinity_body,Time::MIN/4,infinity_body.time){
crate::face_crawler::CrawlResult::Miss(fev)=>Some(fev), crate::face_crawler::CrawlResult::Miss(fev)=>Some(fev),
crate::face_crawler::CrawlResult::Hit(_,_)=>None, crate::face_crawler::CrawlResult::Hit(_,_)=>None,
} }
@ -720,7 +729,7 @@ impl MinkowskiMesh<'_>{
pub fn predict_collision_in(&self,relative_body:&Body,time_limit:Time)->Option<(MinkowskiFace,GigaTime)>{ pub fn predict_collision_in(&self,relative_body:&Body,time_limit:Time)->Option<(MinkowskiFace,GigaTime)>{
self.closest_fev_not_inside(relative_body.clone()).map_or(None,|fev|{ self.closest_fev_not_inside(relative_body.clone()).map_or(None,|fev|{
//continue forwards along the body parabola //continue forwards along the body parabola
match crate::face_crawler::crawl_fev(fev,self,relative_body,relative_body.time,time_limit){ match fev.crawl_fev(self,relative_body,relative_body.time,time_limit){
crate::face_crawler::CrawlResult::Miss(_)=>None, crate::face_crawler::CrawlResult::Miss(_)=>None,
crate::face_crawler::CrawlResult::Hit(face,time)=>Some((face,time)), crate::face_crawler::CrawlResult::Hit(face,time)=>Some((face,time)),
} }
@ -736,7 +745,7 @@ impl MinkowskiMesh<'_>{
); );
self.closest_fev_not_inside(infinity_body).map_or(None,|fev|{ self.closest_fev_not_inside(infinity_body).map_or(None,|fev|{
//continue backwards along the body parabola //continue backwards along the body parabola
match crate::face_crawler::crawl_fev(fev,self,&-relative_body.clone(),-time_limit,-relative_body.time){ match fev.crawl_fev(self,&-relative_body.clone(),-time_limit,-relative_body.time){
crate::face_crawler::CrawlResult::Miss(_)=>None, crate::face_crawler::CrawlResult::Miss(_)=>None,
crate::face_crawler::CrawlResult::Hit(face,time)=>Some((face,-time)),//no need to test -time<time_limit because of the first step crate::face_crawler::CrawlResult::Hit(face,time)=>Some((face,-time)),//no need to test -time<time_limit because of the first step
} }
@ -771,7 +780,7 @@ impl MinkowskiMesh<'_>{
} }
fn infinity_in(&self,infinity_body:Body)->Option<(MinkowskiFace,GigaTime)>{ fn infinity_in(&self,infinity_body:Body)->Option<(MinkowskiFace,GigaTime)>{
let infinity_fev=self.infinity_fev(-infinity_body.velocity,infinity_body.position); let infinity_fev=self.infinity_fev(-infinity_body.velocity,infinity_body.position);
match crate::face_crawler::crawl_fev(infinity_fev,self,&infinity_body,Time::MIN/4,infinity_body.time){ match infinity_fev.crawl_fev(self,&infinity_body,Time::MIN/4,infinity_body.time){
crate::face_crawler::CrawlResult::Miss(_)=>None, crate::face_crawler::CrawlResult::Miss(_)=>None,
crate::face_crawler::CrawlResult::Hit(face,time)=>Some((face,time)), crate::face_crawler::CrawlResult::Hit(face,time)=>Some((face,time)),
} }
@ -787,7 +796,10 @@ impl MinkowskiMesh<'_>{
) )
} }
} }
impl MeshQuery<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert> for MinkowskiMesh<'_>{ impl MeshQuery for MinkowskiMesh<'_>{
type Face=MinkowskiFace;
type Edge=MinkowskiDirectedEdge;
type Vert=MinkowskiVert;
type Normal=Vector3<Fixed<3,96>>; type Normal=Vector3<Fixed<3,96>>;
type Offset=Fixed<4,128>; type Offset=Fixed<4,128>;
// TODO: relative d // TODO: relative d