aidan9382/strafe-client
1
Fork 0
forked from StrafesNET/strafe-client
Code Pull Requests Activity

DirectedEdge trait (huge)

Browse Source
This commit is contained in:
Quaternions 2023-11-16 16:56:34 -08:00
parent 6fcc6b2303
commit 71d221581f
2 changed files with 126 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
src/face_crawler.rs
View File

@ -1,15 +1,15 @@
use crate::physics::Body; use crate::physics::Body;
use crate::model_physics::{FEV,MeshQuery}; use crate::model_physics::{FEV,MeshQuery,DirectedEdge};
use crate::integer::{Time,Planar64}; use crate::integer::{Time,Planar64};
use crate::zeroes::zeroes2; use crate::zeroes::zeroes2;
enum Transition<F,E,V>{ enum Transition<F,E:DirectedEdge,V>{
Miss, Miss,
Next(FEV<F,E,V>,Time), Next(FEV<F,E,V>,Time),
Hit(F,Time), Hit(F,Time),
} }
pub fn next_transition_body<F:Copy,E:Copy,V:Copy>(fev:&FEV<F,E,V>,time:Time,mesh:&impl MeshQuery<F,E,V>,body:&Body,time_limit:Time)->Transition<F,E,V>{ pub fn next_transition_body<F:Copy,E:Copy+DirectedEdge,V:Copy>(fev:&FEV<F,E,V>,time:Time,mesh:&impl MeshQuery<F,E,V>,body:&Body,time_limit:Time)->Transition<F,E,V>{
//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_time=time_limit; let mut best_time=time_limit;
@ -28,18 +28,17 @@ enum Transition<F,E,V>{
} }
} }
//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 &edge_id in mesh.face_edges(face_id).iter(){ for &directed_edge_id in mesh.face_edges(face_id).iter(){
//this needs to be directed edge! let edge_n=mesh.directed_edge_n(directed_edge_id);
todo!(); let n=edge_n.cross(n);
let edge_n=mesh.edge_n(edge_id); let verts=mesh.edge_verts(directed_edge_id.as_undirected());
let n=n.cross(edge_n); let d=n.dot(mesh.vert(verts[0]))+n.dot(mesh.vert(verts[1]));
//picking a vert randomly is terrible //WARNING: d is moved out of the *2 block because of adding two vertices!
let d=n.dot(mesh.vert(mesh.edge_verts(edge_id)[0])); for t in zeroes2(n.dot(body.position)*2-d,n.dot(body.velocity)*2,n.dot(body.acceleration)){
for t in zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
let t=body.time+Time::from(t); let t=body.time+Time::from(t);
if time<t&&t<best_time&&n.dot(body.extrapolated_velocity(t))<Planar64::ZERO{ if time<t&&t<best_time&&n.dot(body.extrapolated_velocity(t))<Planar64::ZERO{
best_time=t; best_time=t;
best_transtition=Transition::Next(FEV::<F,E,V>::Edge(edge_id),t); best_transtition=Transition::Next(FEV::<F,E,V>::Edge(directed_edge_id.as_undirected()),t);
break; break;
} }
} }
@ -49,22 +48,27 @@ enum Transition<F,E,V>{
&FEV::<F,E,V>::Edge(edge_id)=>{ &FEV::<F,E,V>::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);
for &test_face_id in mesh.edge_faces(edge_id).iter(){ for (i,&edge_face_id) in mesh.edge_faces(edge_id).iter().enumerate(){
let face_n=mesh.face_nd(test_face_id).0; let face_n=mesh.face_nd(edge_face_id).0;
let n=edge_n.cross(face_n); //edge_n gets parity from the order of edge_faces
let d=n.dot(mesh.vert(mesh.edge_verts(edge_id)[0])); let n=edge_n.cross(face_n)*((i as i64)*2-1);
for t in zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ let verts=mesh.edge_verts(edge_id);
let d=n.dot(mesh.vert(verts[0]))+n.dot(mesh.vert(verts[1]));
//WARNING yada yada d *2
for t in zeroes2((n.dot(body.position))*2-d,n.dot(body.velocity)*2,n.dot(body.acceleration)){
let t=body.time+Time::from(t); let t=body.time+Time::from(t);
if time<t&&t<best_time&&n.dot(body.extrapolated_velocity(t))<Planar64::ZERO{ if time<t&&t<best_time&&n.dot(body.extrapolated_velocity(t))<Planar64::ZERO{
best_time=t; best_time=t;
best_transtition=Transition::Next(FEV::<F,E,V>::Face(test_face_id),t); best_transtition=Transition::Next(FEV::<F,E,V>::Face(edge_face_id),t);
break; break;
} }
} }
} }
//test each vertex collision time, ignoring roots with zero or conflicting derivative //test each vertex collision time, ignoring roots with zero or conflicting derivative
let n=mesh.edge_n(edge_id); let n=mesh.edge_n(edge_id);
for &vert_id in mesh.edge_verts(edge_id).iter(){ for (i,&vert_id) in mesh.edge_verts(edge_id).iter().enumerate(){
//vertex normal gets parity from vert index
let n=n*(1-2*(i as i64));
let d=n.dot(mesh.vert(vert_id)); let d=n.dot(mesh.vert(vert_id));
for t in zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ for t in zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
let t=body.time+Time::from(t); let t=body.time+Time::from(t);
@ -79,14 +83,15 @@ enum Transition<F,E,V>{
}, },
&FEV::<F,E,V>::Vert(vert_id)=>{ &FEV::<F,E,V>::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 &edge_id in mesh.vert_edges(vert_id).iter(){ for &directed_edge_id in mesh.vert_edges(vert_id).iter(){
let n=mesh.edge_n(edge_id); //edge is directed away from vertex, but we want the dot product to turn out negative
let n=-mesh.directed_edge_n(directed_edge_id);
let d=n.dot(mesh.vert(vert_id)); let d=n.dot(mesh.vert(vert_id));
for t in zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ for t in zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
let t=body.time+Time::from(t); let t=body.time+Time::from(t);
if time<t&&t<best_time&&n.dot(body.extrapolated_velocity(t))<Planar64::ZERO{ if time<t&&t<best_time&&n.dot(body.extrapolated_velocity(t))<Planar64::ZERO{
best_time=t; best_time=t;
best_transtition=Transition::Next(FEV::<F,E,V>::Edge(edge_id),t); best_transtition=Transition::Next(FEV::<F,E,V>::Edge(directed_edge_id.as_undirected()),t);
break; break;
} }
} }
@ -96,7 +101,7 @@ enum Transition<F,E,V>{
} }
best_transtition best_transtition
} }
pub fn crawl_fev_body<F:Copy,E:Copy,V:Copy>(mut fev:FEV<F,E,V>,mesh:&impl MeshQuery<F,E,V>,relative_body:&Body,time_limit:Time)->Option<(F,Time)>{ pub fn crawl_fev_body<F:Copy,E:Copy+DirectedEdge,V:Copy>(mut fev:FEV<F,E,V>,mesh:&impl MeshQuery<F,E,V>,relative_body:&Body,time_limit:Time)->Option<(F,Time)>{
let mut time=relative_body.time; let mut time=relative_body.time;
loop{ loop{
match next_transition_body(&fev,time,mesh,relative_body,time_limit){ match next_transition_body(&fev,time,mesh,relative_body,time_limit){

183
src/model_physics.rs
View File

@ -6,28 +6,34 @@ pub struct VertId(usize);
#[derive(Debug,Clone,Copy,Hash,Eq,PartialEq)] #[derive(Debug,Clone,Copy,Hash,Eq,PartialEq)]
pub struct EdgeId(usize); pub struct EdgeId(usize);
impl EdgeId{ impl EdgeId{
fn as_directed_edge_id(&self,parity:bool)->DirectedEdgeId{ fn as_directed(&self,parity:bool)->DirectedEdgeId{
DirectedEdgeId(self.0|((parity as usize)<<(usize::BITS-1))) DirectedEdgeId(self.0|((parity as usize)<<(usize::BITS-1)))
} }
} }
pub trait DirectedEdge{
type UndirectedEdge;
fn as_undirected(&self)->Self::UndirectedEdge;
fn parity(&self)->bool;
}
/// DirectedEdgeId refers to an EdgeId when undirected. /// DirectedEdgeId refers to an EdgeId when undirected.
#[derive(Debug,Clone,Copy,Hash,Eq,PartialEq)] #[derive(Debug,Clone,Copy,Hash,Eq,PartialEq)]
pub struct DirectedEdgeId(usize); pub struct DirectedEdgeId(usize);
impl DirectedEdgeId{ impl DirectedEdge for DirectedEdgeId{
fn as_edge_id(&self)->EdgeId{ type UndirectedEdge=EdgeId;
fn as_undirected(&self)->EdgeId{
EdgeId(self.0&!(1<<(usize::BITS-1))) EdgeId(self.0&!(1<<(usize::BITS-1)))
} }
fn signum(&self)->isize{ fn parity(&self)->bool{
((self.0&(1<<(usize::BITS-1))!=0) as isize)*2-1 self.0&(1<<(usize::BITS-1))!=0
} }
} }
#[derive(Debug,Clone,Copy,Hash,Eq,PartialEq)] #[derive(Debug,Clone,Copy,Hash,Eq,PartialEq)]
pub struct FaceId(usize); pub struct FaceId(usize);
//Vertex <-> Edge <-> Face -> Collide //Vertex <-> Edge <-> Face -> Collide
pub enum FEV<F,E,V>{ pub enum FEV<F,E:DirectedEdge,V>{
Face(F), Face(F),
Edge(E), Edge(E::UndirectedEdge),
Vert(V), Vert(V),
} }
@ -42,8 +48,25 @@ impl Face{
} }
} }
struct Vert(Planar64Vec3); struct Vert(Planar64Vec3);
pub trait MeshQuery<FACE:Clone,EDGE:Clone+DirectedEdge,VERT:Clone>{
fn edge_n(&self,edge_id:EDGE::UndirectedEdge)->Planar64Vec3{
let verts=self.edge_verts(edge_id);
self.vert(verts[1].clone())-self.vert(verts[0].clone())
}
fn directed_edge_n(&self,directed_edge_id:EDGE)->Planar64Vec3{
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)
}
fn vert(&self,vert_id:VERT)->Planar64Vec3;
fn face_nd(&self,face_id:FACE)->(Planar64Vec3,Planar64);
fn face_edges(&self,face_id:FACE)->Cow<Vec<EDGE>>;
fn edge_faces(&self,edge_id:EDGE::UndirectedEdge)->Cow<[FACE;2]>;
fn edge_verts(&self,edge_id:EDGE::UndirectedEdge)->Cow<[VERT;2]>;
fn vert_edges(&self,vert_id:VERT)->Cow<Vec<EDGE>>;
fn vert_faces(&self,vert_id:VERT)->Cow<Vec<FACE>>;
}
struct FaceRefs{ struct FaceRefs{
edges:Vec<EdgeId>, edges:Vec<DirectedEdgeId>,
//verts:Vec<VertId>, //verts:Vec<VertId>,
} }
struct EdgeRefs{ struct EdgeRefs{
@ -87,7 +110,7 @@ impl EdgeRefGuy{
self.0[i]=face_id; self.0[i]=face_id;
} }
} }
struct FaceRefGuy(Vec<EdgeId>); struct FaceRefGuy(Vec<DirectedEdgeId>);
#[derive(Default)] #[derive(Default)]
struct EdgePool{ struct EdgePool{
edge_guys:Vec<(EdgeIdGuy,EdgeRefGuy)>, edge_guys:Vec<(EdgeIdGuy,EdgeRefGuy)>,
@ -139,12 +162,12 @@ impl From<&crate::model::IndexedModel> for PhysicsMesh{
//index edges & face into vertices //index edges & face into vertices
{ {
let vert_ref_guy=unsafe{vert_ref_guys.get_unchecked_mut(vert0_id)}; let vert_ref_guy=unsafe{vert_ref_guys.get_unchecked_mut(vert0_id)};
vert_ref_guy.edges.insert(edge_id.as_directed_edge_id(!is_sorted)); vert_ref_guy.edges.insert(edge_id.as_directed(!is_sorted));
vert_ref_guy.faces.insert(face_id); vert_ref_guy.faces.insert(face_id);
unsafe{vert_ref_guys.get_unchecked_mut(vert1_id)}.edges.insert(edge_id.as_directed_edge_id(is_sorted)); unsafe{vert_ref_guys.get_unchecked_mut(vert1_id)}.edges.insert(edge_id.as_directed(is_sorted));
} }
//return edge_id //return directed_edge_id
edge_id edge_id.as_directed(is_sorted)
}).collect(); }).collect();
//choose precision loss randomly idk //choose precision loss randomly idk
normal=normal/len as i64; normal=normal/len as i64;
@ -176,32 +199,12 @@ impl From<&crate::model::IndexedModel> for PhysicsMesh{
} }
} }
pub trait MeshQuery<FACE:Clone,EDGE:Clone,VERT:Clone>{
fn edge_n(&self,edge_id:EDGE)->Planar64Vec3{
let verts=self.edge_verts(edge_id);
self.vert(verts[1].clone())-self.vert(verts[0].clone())
}
fn vert(&self,vert_id:VERT)->Planar64Vec3;
fn face_nd(&self,face_id:FACE)->(Planar64Vec3,Planar64);
fn face_edges(&self,face_id:FACE)->Cow<Vec<EDGE>>;
fn edge_faces(&self,edge_id:EDGE)->Cow<[FACE;2]>;
fn edge_verts(&self,edge_id:EDGE)->Cow<[VERT;2]>;
fn vert_edges(&self,vert_id:VERT)->Cow<Vec<EDGE>>;
fn vert_faces(&self,vert_id:VERT)->Cow<Vec<FACE>>;
}
impl PhysicsMesh{ impl PhysicsMesh{
pub fn verts<'a>(&'a self)->impl Iterator<Item=Planar64Vec3>+'a{ pub fn verts<'a>(&'a self)->impl Iterator<Item=Planar64Vec3>+'a{
self.verts.iter().map(|Vert(pos)|*pos) self.verts.iter().map(|Vert(pos)|*pos)
} }
fn vert_directed_edges(&self,vert_id:VertId)->Cow<Vec<DirectedEdgeId>>{
Cow::Borrowed(&self.vert_topology[vert_id.0].edges)
}
fn directed_edge_n(&self,directed_edge_id:DirectedEdgeId)->Planar64Vec3{
let verts=self.edge_verts(directed_edge_id.as_edge_id());
(self.vert(verts[1].clone())-self.vert(verts[0].clone()))*(directed_edge_id.signum() as i64)
}
} }
impl MeshQuery<FaceId,EdgeId,VertId> for PhysicsMesh{ impl MeshQuery<FaceId,DirectedEdgeId,VertId> for PhysicsMesh{
fn face_nd(&self,face_id:FaceId)->(Planar64Vec3,Planar64){ fn face_nd(&self,face_id:FaceId)->(Planar64Vec3,Planar64){
(self.faces[face_id.0].normal,self.faces[face_id.0].dot) (self.faces[face_id.0].normal,self.faces[face_id.0].dot)
} }
@ -209,7 +212,7 @@ impl MeshQuery<FaceId,EdgeId,VertId> for PhysicsMesh{
fn vert(&self,vert_id:VertId)->Planar64Vec3{ fn vert(&self,vert_id:VertId)->Planar64Vec3{
self.verts[vert_id.0].0 self.verts[vert_id.0].0
} }
fn face_edges(&self,face_id:FaceId)->Cow<Vec<EdgeId>>{ fn face_edges(&self,face_id:FaceId)->Cow<Vec<DirectedEdgeId>>{
Cow::Borrowed(&self.face_topology[face_id.0].edges) Cow::Borrowed(&self.face_topology[face_id.0].edges)
} }
fn edge_faces(&self,edge_id:EdgeId)->Cow<[FaceId;2]>{ fn edge_faces(&self,edge_id:EdgeId)->Cow<[FaceId;2]>{
@ -218,9 +221,8 @@ impl MeshQuery<FaceId,EdgeId,VertId> for PhysicsMesh{
fn edge_verts(&self,edge_id:EdgeId)->Cow<[VertId;2]>{ fn edge_verts(&self,edge_id:EdgeId)->Cow<[VertId;2]>{
Cow::Borrowed(&self.edge_topology[edge_id.0].verts) Cow::Borrowed(&self.edge_topology[edge_id.0].verts)
} }
fn vert_edges(&self,vert_id:VertId)->Cow<Vec<EdgeId>>{ fn vert_edges(&self,vert_id:VertId)->Cow<Vec<DirectedEdgeId>>{
//not poggers Cow::Borrowed(&self.vert_topology[vert_id.0].edges)
Cow::Owned(self.vert_topology[vert_id.0].edges.iter().map(|directed_edge_id|directed_edge_id.as_edge_id()).collect())
} }
fn vert_faces(&self,vert_id:VertId)->Cow<Vec<FaceId>>{ fn vert_faces(&self,vert_id:VertId)->Cow<Vec<FaceId>>{
Cow::Borrowed(&self.vert_topology[vert_id.0].faces) Cow::Borrowed(&self.vert_topology[vert_id.0].faces)
@ -257,16 +259,8 @@ impl TransformedMesh<'_>{
} }
best_vert best_vert
} }
#[inline]
fn vert_directed_edges(&self,vert_id:VertId)->Cow<Vec<DirectedEdgeId>>{
self.mesh.vert_directed_edges(vert_id)
}
#[inline]
fn directed_edge_n(&self,directed_edge_id:DirectedEdgeId)->Planar64Vec3{
self.mesh.directed_edge_n(directed_edge_id)
}
} }
impl MeshQuery<FaceId,EdgeId,VertId> for TransformedMesh<'_>{ impl MeshQuery<FaceId,DirectedEdgeId,VertId> for TransformedMesh<'_>{
fn face_nd(&self,face_id:FaceId)->(Planar64Vec3,Planar64){ fn face_nd(&self,face_id:FaceId)->(Planar64Vec3,Planar64){
let (n,d)=self.mesh.face_nd(face_id); let (n,d)=self.mesh.face_nd(face_id);
let transformed_n=*self.normal_transform*n; let transformed_n=*self.normal_transform*n;
@ -277,7 +271,7 @@ impl MeshQuery<FaceId,EdgeId,VertId> for TransformedMesh<'_>{
self.transform.transform_point3(self.mesh.vert(vert_id)) self.transform.transform_point3(self.mesh.vert(vert_id))
} }
#[inline] #[inline]
fn face_edges(&self,face_id:FaceId)->Cow<Vec<EdgeId>>{ fn face_edges(&self,face_id:FaceId)->Cow<Vec<DirectedEdgeId>>{
self.mesh.face_edges(face_id) self.mesh.face_edges(face_id)
} }
#[inline] #[inline]
@ -289,7 +283,7 @@ impl MeshQuery<FaceId,EdgeId,VertId> for TransformedMesh<'_>{
self.mesh.edge_verts(edge_id) self.mesh.edge_verts(edge_id)
} }
#[inline] #[inline]
fn vert_edges(&self,vert_id:VertId)->Cow<Vec<EdgeId>>{ fn vert_edges(&self,vert_id:VertId)->Cow<Vec<DirectedEdgeId>>{
self.mesh.vert_edges(vert_id) self.mesh.vert_edges(vert_id)
} }
#[inline] #[inline]
@ -307,15 +301,36 @@ enum MinkowskiVert{
VertVert(VertId,VertId), VertVert(VertId,VertId),
} }
#[derive(Clone,Copy)] #[derive(Clone,Copy)]
enum MinkowskiEdge{ pub enum MinkowskiEdge{
VertEdge(VertId,EdgeId), VertEdge(VertId,EdgeId),
EdgeVert(EdgeId,VertId), EdgeVert(EdgeId,VertId),
//EdgeEdge when edges are parallel //EdgeEdge when edges are parallel
} }
#[derive(Clone,Copy)]
enum MinkowskiDirectedEdge{
VertEdge(VertId,DirectedEdgeId),
EdgeVert(DirectedEdgeId,VertId),
//EdgeEdge when edges are parallel
}
impl DirectedEdge for MinkowskiDirectedEdge{
type UndirectedEdge=MinkowskiEdge;
fn as_undirected(&self)->Self::UndirectedEdge{
match self{
MinkowskiDirectedEdge::VertEdge(v0,e1)=>MinkowskiEdge::VertEdge(*v0,e1.as_undirected()),
MinkowskiDirectedEdge::EdgeVert(e0,v1)=>MinkowskiEdge::EdgeVert(e0.as_undirected(),*v1),
}
}
fn parity(&self)->bool{
match self{
MinkowskiDirectedEdge::VertEdge(_,e)
|MinkowskiDirectedEdge::EdgeVert(e,_)=>e.parity(),
}
}
}
#[derive(Debug,Clone,Copy,Hash,Eq,PartialEq)] #[derive(Debug,Clone,Copy,Hash,Eq,PartialEq)]
pub enum MinkowskiFace{ pub enum MinkowskiFace{
VertFace(VertId,FaceId), VertFace(VertId,FaceId),
EdgeEdge(EdgeId,EdgeId), EdgeEdge(EdgeId,EdgeId,bool),
FaceVert(FaceId,VertId), FaceVert(FaceId,VertId),
//EdgeFace //EdgeFace
//FaceEdge //FaceEdge
@ -337,11 +352,11 @@ impl MinkowskiMesh<'_>{
fn farthest_vert(&self,dir:Planar64Vec3)->MinkowskiVert{ fn farthest_vert(&self,dir:Planar64Vec3)->MinkowskiVert{
MinkowskiVert::VertVert(self.mesh0.farthest_vert(dir),self.mesh1.farthest_vert(-dir)) MinkowskiVert::VertVert(self.mesh0.farthest_vert(dir),self.mesh1.farthest_vert(-dir))
} }
fn closest_fev(&self,point:Planar64Vec3)->FEV<MinkowskiFace,MinkowskiEdge,MinkowskiVert>{ fn closest_fev(&self,point:Planar64Vec3)->FEV<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>{
//put some genius code right here instead of this //put some genius code right here instead of this
//assume that point is outside the mesh and nonzero //assume that point is outside the mesh and nonzero
//find vertex on mesh0 farthest in point direction //find vertex on mesh0 farthest in point direction
let fev=FEV::<MinkowskiFace,MinkowskiEdge,MinkowskiVert>::Vert(self.farthest_vert(point)); let fev=FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>::Vert(self.farthest_vert(point));
crate::face_crawler::crawl_fev_dot(fev,self,point) crate::face_crawler::crawl_fev_dot(fev,self,point)
} }
pub fn predict_collision_in(&self,relative_body:&crate::physics::Body,time_limit:crate::integer::Time)->Option<(MinkowskiFace,crate::integer::Time)>{ pub fn predict_collision_in(&self,relative_body:&crate::physics::Body,time_limit:crate::integer::Time)->Option<(MinkowskiFace,crate::integer::Time)>{
@ -353,31 +368,32 @@ impl MinkowskiMesh<'_>{
let mut best_time=time_limit; let mut best_time=time_limit;
let mut best_edge=None; let mut best_edge=None;
let face_n=self.face_nd(contact_face_id).0; let face_n=self.face_nd(contact_face_id).0;
for &edge_id in self.face_edges(contact_face_id).iter(){ for &directed_edge_id in self.face_edges(contact_face_id).iter(){
let edge_n=self.edge_n(edge_id); let edge_n=self.directed_edge_n(directed_edge_id);
let n=face_n.cross(edge_n); let n=face_n.cross(edge_n);
//picking a vert randomly is terrible let verts=self.edge_verts(directed_edge_id.as_undirected());
let d=n.dot(self.vert(self.edge_verts(edge_id)[0])); let d=n.dot(self.vert(verts[0]))+n.dot(self.vert(verts[1]));
for t in crate::zeroes::zeroes2((n.dot(relative_body.position)-d)*2,n.dot(relative_body.velocity)*2,n.dot(relative_body.acceleration)){ //WARNING! d outside of *2
for t in crate::zeroes::zeroes2((n.dot(relative_body.position))*2-d,n.dot(relative_body.velocity)*2,n.dot(relative_body.acceleration)){
let t=relative_body.time+crate::integer::Time::from(t); let t=relative_body.time+crate::integer::Time::from(t);
if relative_body.time<t&&t<best_time&&n.dot(relative_body.extrapolated_velocity(t))<Planar64::ZERO{ if relative_body.time<t&&t<best_time&&n.dot(relative_body.extrapolated_velocity(t))<Planar64::ZERO{
best_time=t; best_time=t;
best_edge=Some(edge_id); best_edge=Some(directed_edge_id);
break; break;
} }
} }
} }
best_edge.map(|e|(e,best_time)) best_edge.map(|e|(e.as_undirected(),best_time))
} }
} }
impl MeshQuery<MinkowskiFace,MinkowskiEdge,MinkowskiVert> for MinkowskiMesh<'_>{ impl MeshQuery<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert> for MinkowskiMesh<'_>{
fn face_nd(&self,face_id:MinkowskiFace)->(Planar64Vec3,Planar64){ fn face_nd(&self,face_id:MinkowskiFace)->(Planar64Vec3,Planar64){
match face_id{ match face_id{
MinkowskiFace::VertFace(v0,f1)=>{ MinkowskiFace::VertFace(v0,f1)=>{
let (n,d)=self.mesh1.face_nd(f1); let (n,d)=self.mesh1.face_nd(f1);
(-n,d-n.dot(self.mesh0.vert(v0))) (-n,d-n.dot(self.mesh0.vert(v0)))
}, },
MinkowskiFace::EdgeEdge(e0,e1)=>{ MinkowskiFace::EdgeEdge(e0,e1,parity)=>{
let edge0_n=self.mesh0.edge_n(e0); let edge0_n=self.mesh0.edge_n(e0);
let edge1_n=self.mesh1.edge_n(e1); let edge1_n=self.mesh1.edge_n(e1);
let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).borrow(); let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).borrow();
@ -385,8 +401,7 @@ impl MeshQuery<MinkowskiFace,MinkowskiEdge,MinkowskiVert> for MinkowskiMesh<'_>{
let n=edge0_n.cross(edge1_n); let n=edge0_n.cross(edge1_n);
let e0d=n.dot(self.mesh0.vert(e0v0)+self.mesh0.vert(e0v1)); let e0d=n.dot(self.mesh0.vert(e0v0)+self.mesh0.vert(e0v1));
let e1d=n.dot(self.mesh0.vert(e1v0)+self.mesh0.vert(e1v1)); let e1d=n.dot(self.mesh0.vert(e1v0)+self.mesh0.vert(e1v1));
let sign=e0d.signum_i64(); (n*(parity as i64*4-2),(e0d-e1d)*(parity as i64*2-1))
(n*(sign*2),(e0d-e1d)*sign)
}, },
MinkowskiFace::FaceVert(f0,v1)=>{ MinkowskiFace::FaceVert(f0,v1)=>{
let (n,d)=self.mesh0.face_nd(f0); let (n,d)=self.mesh0.face_nd(f0);
@ -401,27 +416,27 @@ impl MeshQuery<MinkowskiFace,MinkowskiEdge,MinkowskiVert> for MinkowskiMesh<'_>{
}, },
} }
} }
fn face_edges(&self,face_id:MinkowskiFace)->Cow<Vec<MinkowskiEdge>>{ fn face_edges(&self,face_id:MinkowskiFace)->Cow<Vec<MinkowskiDirectedEdge>>{
match face_id{ match face_id{
MinkowskiFace::VertFace(v0,f1)=>{ MinkowskiFace::VertFace(v0,f1)=>{
Cow::Owned(self.mesh1.face_edges(f1).iter().map(|&edge_id1|{ Cow::Owned(self.mesh1.face_edges(f1).iter().map(|&edge_id1|{
MinkowskiEdge::VertEdge(v0,edge_id1) MinkowskiDirectedEdge::VertEdge(v0,edge_id1)
}).collect()) }).collect())
}, },
MinkowskiFace::EdgeEdge(e0,e1)=>{ MinkowskiFace::EdgeEdge(e0,e1,parity)=>{
let e0v=self.mesh0.edge_verts(e0); let e0v=self.mesh0.edge_verts(e0);
let e1v=self.mesh1.edge_verts(e1); let e1v=self.mesh1.edge_verts(e1);
//could sort this if ordered edges are needed //could sort this if ordered edges are needed
Cow::Owned(vec![ Cow::Owned(vec![
MinkowskiEdge::VertEdge(e0v[0],e1), MinkowskiDirectedEdge::VertEdge(e0v[0],e1.as_directed(parity)),
MinkowskiEdge::VertEdge(e0v[1],e1), MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),e1v[0]),
MinkowskiEdge::EdgeVert(e0,e1v[0]), MinkowskiDirectedEdge::VertEdge(e0v[1],e1.as_directed(!parity)),
MinkowskiEdge::EdgeVert(e0,e1v[1]), MinkowskiDirectedEdge::EdgeVert(e0.as_directed(!parity),e1v[1]),
]) ])
}, },
MinkowskiFace::FaceVert(f0,v1)=>{ MinkowskiFace::FaceVert(f0,v1)=>{
Cow::Owned(self.mesh0.face_edges(f0).iter().map(|&edge_id0|{ Cow::Owned(self.mesh0.face_edges(f0).iter().map(|&edge_id0|{
MinkowskiEdge::EdgeVert(edge_id0,v1) MinkowskiDirectedEdge::EdgeVert(edge_id0,v1)
}).collect()) }).collect())
}, },
} }
@ -430,13 +445,12 @@ impl MeshQuery<MinkowskiFace,MinkowskiEdge,MinkowskiVert> for MinkowskiMesh<'_>{
match edge_id{ match edge_id{
MinkowskiEdge::VertEdge(v0,e1)=>{ MinkowskiEdge::VertEdge(v0,e1)=>{
let e1f=self.mesh1.edge_faces(e1); let e1f=self.mesh1.edge_faces(e1);
Cow::Owned([(e1f[0],e1f[1]),(e1f[1],e1f[0])].map(|(edge_face_id1,other_edge_face_id1)|{ Cow::Owned([(e1f[0],e1f[1],true),(e1f[1],e1f[0],false)].map(|(edge_face_id1,other_edge_face_id1,face_parity)|{
let mut best_edge=None; let mut best_edge=None;
let mut best_d=Planar64::MAX; let mut best_d=Planar64::MAX;
let edge_face1_n=self.mesh1.face_nd(edge_face_id1).0; let edge_face1_n=self.mesh1.face_nd(edge_face_id1).0;
let other_edge_face1_n=self.mesh1.face_nd(other_edge_face_id1).0; let other_edge_face1_n=self.mesh1.face_nd(other_edge_face_id1).0;
let v0e=self.mesh0.vert_directed_edges(v0); for &directed_edge_id0 in self.mesh0.vert_edges(v0).iter(){
for &directed_edge_id0 in v0e.iter(){
let edge0_n=self.mesh0.directed_edge_n(directed_edge_id0); let edge0_n=self.mesh0.directed_edge_n(directed_edge_id0);
if edge_face1_n.dot(edge0_n)<Planar64::ZERO{ if edge_face1_n.dot(edge0_n)<Planar64::ZERO{
let d=other_edge_face1_n.dot(edge0_n); let d=other_edge_face1_n.dot(edge0_n);
@ -448,19 +462,18 @@ impl MeshQuery<MinkowskiFace,MinkowskiEdge,MinkowskiVert> for MinkowskiMesh<'_>{
} }
best_edge.map_or( best_edge.map_or(
MinkowskiFace::VertFace(v0,edge_face_id1), MinkowskiFace::VertFace(v0,edge_face_id1),
|directed_edge_id0|MinkowskiFace::EdgeEdge(directed_edge_id0.as_edge_id(),e1) |directed_edge_id0|MinkowskiFace::EdgeEdge(directed_edge_id0.as_undirected(),e1,directed_edge_id0.parity()^face_parity)
) )
})) }))
}, },
MinkowskiEdge::EdgeVert(e0,v1)=>{ MinkowskiEdge::EdgeVert(e0,v1)=>{
let e0f=self.mesh0.edge_faces(e0); let e0f=self.mesh0.edge_faces(e0);
Cow::Owned([(e0f[0],e0f[1]),(e0f[1],e0f[0])].map(|(edge_face_id0,other_edge_face_id0)|{ Cow::Owned([(e0f[0],e0f[1],true),(e0f[1],e0f[0],false)].map(|(edge_face_id0,other_edge_face_id0,face_parity)|{
let mut best_edge=None; let mut best_edge=None;
let mut best_d=Planar64::MAX; let mut best_d=Planar64::MAX;
let edge_face0_n=self.mesh0.face_nd(edge_face_id0).0; let edge_face0_n=self.mesh0.face_nd(edge_face_id0).0;
let other_edge_face0_n=self.mesh0.face_nd(other_edge_face_id0).0; let other_edge_face0_n=self.mesh0.face_nd(other_edge_face_id0).0;
let v1e=self.mesh1.vert_directed_edges(v1); for &directed_edge_id1 in self.mesh1.vert_edges(v1).iter(){
for &directed_edge_id1 in v1e.iter(){
let edge1_n=self.mesh1.directed_edge_n(directed_edge_id1); let edge1_n=self.mesh1.directed_edge_n(directed_edge_id1);
if edge_face0_n.dot(edge1_n)<Planar64::ZERO{ if edge_face0_n.dot(edge1_n)<Planar64::ZERO{
let d=other_edge_face0_n.dot(edge1_n); let d=other_edge_face0_n.dot(edge1_n);
@ -472,7 +485,7 @@ impl MeshQuery<MinkowskiFace,MinkowskiEdge,MinkowskiVert> for MinkowskiMesh<'_>{
} }
best_edge.map_or( best_edge.map_or(
MinkowskiFace::FaceVert(edge_face_id0,v1), MinkowskiFace::FaceVert(edge_face_id0,v1),
|directed_edge_id1|MinkowskiFace::EdgeEdge(e0,directed_edge_id1.as_edge_id()) |directed_edge_id1|MinkowskiFace::EdgeEdge(e0,directed_edge_id1.as_undirected(),directed_edge_id1.parity()^face_parity)
) )
})) }))
}, },
@ -492,24 +505,24 @@ impl MeshQuery<MinkowskiFace,MinkowskiEdge,MinkowskiVert> for MinkowskiMesh<'_>{
}, },
} }
} }
fn vert_edges(&self,vert_id:MinkowskiVert)->Cow<Vec<MinkowskiEdge>>{ fn vert_edges(&self,vert_id:MinkowskiVert)->Cow<Vec<MinkowskiDirectedEdge>>{
match vert_id{ match vert_id{
MinkowskiVert::VertVert(v0,v1)=>{ MinkowskiVert::VertVert(v0,v1)=>{
let mut edges=Vec::new(); let mut edges=Vec::new();
let v0e=self.mesh0.vert_directed_edges(v0); let v0e=self.mesh0.vert_edges(v0);
let v1f=self.mesh1.vert_faces(v1); let v1f=self.mesh1.vert_faces(v1);
for &directed_edge_id in v0e.iter(){ for &directed_edge_id in v0e.iter(){
let n=self.mesh0.directed_edge_n(directed_edge_id); let n=self.mesh0.directed_edge_n(directed_edge_id);
if v1f.iter().all(|&face_id|n.dot(self.mesh1.face_nd(face_id).0)<Planar64::ZERO){ if v1f.iter().all(|&face_id|n.dot(self.mesh1.face_nd(face_id).0)<Planar64::ZERO){
edges.push(MinkowskiEdge::EdgeVert(directed_edge_id.as_edge_id(),v1)); edges.push(MinkowskiDirectedEdge::EdgeVert(directed_edge_id,v1));
} }
} }
let v1e=self.mesh1.vert_directed_edges(v1); let v1e=self.mesh1.vert_edges(v1);
let v0f=self.mesh0.vert_faces(v0); let v0f=self.mesh0.vert_faces(v0);
for &directed_edge_id in v1e.iter(){ for &directed_edge_id in v1e.iter(){
let n=self.mesh1.directed_edge_n(directed_edge_id); let n=self.mesh1.directed_edge_n(directed_edge_id);
if v0f.iter().all(|&face_id|n.dot(self.mesh0.face_nd(face_id).0)<Planar64::ZERO){ if v0f.iter().all(|&face_id|n.dot(self.mesh0.face_nd(face_id).0)<Planar64::ZERO){
edges.push(MinkowskiEdge::VertEdge(v0,directed_edge_id.as_edge_id())); edges.push(MinkowskiDirectedEdge::VertEdge(v0,directed_edge_id));
} }
} }
Cow::Owned(edges) Cow::Owned(edges)