diff --git a/src/face_crawler.rs b/src/face_crawler.rs index 27df013..cae28bb 100644 --- a/src/face_crawler.rs +++ b/src/face_crawler.rs @@ -1,7 +1,6 @@ use crate::physics::Body; use crate::model_physics::{FEV,MeshQuery,DirectedEdge}; -use strafesnet_common::integer::{Time,Planar64}; -use strafesnet_common::zeroes::zeroes2; +use strafesnet_common::integer::{Time,Fixed,Planar64}; enum Transition{ Miss, @@ -22,9 +21,9 @@ enum Transition{ let (n,d)=mesh.face_nd(face_id); //TODO: use higher precision d value? //use the mesh transform translation instead of baking it into the d value. - for t in zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ + for t in Fixed::<2,64>::zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ let t=body.time+Time::from(t); - if time<=t&&t{ //WARNING: d is moved out of the *2 block because of adding two vertices! for t in zeroes2(n.dot(body.position*2-(mesh.vert(verts[0])+mesh.vert(verts[1]))),n.dot(body.velocity)*2,n.dot(body.acceleration)){ let t=body.time+Time::from(t); - if time<=t&&t::Edge(directed_edge_id.as_undirected()),t); break; @@ -59,7 +58,7 @@ enum Transition{ //WARNING yada yada d *2 for t in zeroes2(n.dot(delta_pos),n.dot(body.velocity)*2,n.dot(body.acceleration)){ let t=body.time+Time::from(t); - if time<=t&&t::Face(edge_face_id),t); break; @@ -72,7 +71,7 @@ enum Transition{ let n=edge_n*(1-2*(i as i64)); for t in zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ let t=body.time+Time::from(t); - if time<=t&&t::Vert(vert_id),t); break; @@ -88,7 +87,7 @@ enum Transition{ let n=-mesh.directed_edge_n(directed_edge_id); for t in zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ let t=body.time+Time::from(t); - if time<=t&&t::Edge(directed_edge_id.as_undirected()),t); break; @@ -104,7 +103,7 @@ pub enum CrawlResult{ Miss(FEV), Hit(F,Time), } -pub fn crawl_fev(mut fev:FEV,mesh:&impl MeshQuery,relative_body:&Body,start_time:Time,time_limit:Time)->CrawlResult{ +pub fn crawl_fev(mut fev:FEV,mesh:&impl MeshQuery,relative_body:&Body,start_time:Time,time_limit:Time)->CrawlResult{ let mut time=start_time; for _ in 0..20{ match next_transition(&fev,time,mesh,relative_body,time_limit){ diff --git a/src/model_physics.rs b/src/model_physics.rs index dc9e342..e99d44e 100644 --- a/src/model_physics.rs +++ b/src/model_physics.rs @@ -1,8 +1,7 @@ use std::borrow::{Borrow,Cow}; use std::collections::{HashSet,HashMap}; use strafesnet_common::model::{self,MeshId,PolygonIter}; -use strafesnet_common::zeroes; -use strafesnet_common::integer::{self,Planar64,Planar64Vec3}; +use strafesnet_common::integer::{self,vec3,Fixed,Planar64,Planar64Vec3}; pub trait UndirectedEdge{ type DirectedEdge:Copy+DirectedEdge; @@ -137,22 +136,22 @@ impl PhysicsMesh{ //go go gadget debug print mesh let data=PhysicsMeshData{ faces:vec![ - Face{normal:Planar64Vec3::raw_xyz( 4294967296, 0, 0),dot:Planar64::raw(4294967296)}, - Face{normal:Planar64Vec3::raw_xyz( 0, 4294967296, 0),dot:Planar64::raw(4294967296)}, - Face{normal:Planar64Vec3::raw_xyz( 0, 0, 4294967296),dot:Planar64::raw(4294967296)}, - Face{normal:Planar64Vec3::raw_xyz(-4294967296, 0, 0),dot:Planar64::raw(4294967296)}, - Face{normal:Planar64Vec3::raw_xyz( 0,-4294967296, 0),dot:Planar64::raw(4294967296)}, - Face{normal:Planar64Vec3::raw_xyz( 0, 0,-4294967296),dot:Planar64::raw(4294967296)} + Face{normal:vec3::raw_xyz( 4294967296, 0, 0),dot:Planar64::raw(4294967296)}, + Face{normal:vec3::raw_xyz( 0, 4294967296, 0),dot:Planar64::raw(4294967296)}, + Face{normal:vec3::raw_xyz( 0, 0, 4294967296),dot:Planar64::raw(4294967296)}, + Face{normal:vec3::raw_xyz(-4294967296, 0, 0),dot:Planar64::raw(4294967296)}, + Face{normal:vec3::raw_xyz( 0,-4294967296, 0),dot:Planar64::raw(4294967296)}, + Face{normal:vec3::raw_xyz( 0, 0,-4294967296),dot:Planar64::raw(4294967296)} ], verts:vec![ - Vert(Planar64Vec3::raw_xyz( 4294967296,-4294967296,-4294967296)), - Vert(Planar64Vec3::raw_xyz( 4294967296, 4294967296,-4294967296)), - Vert(Planar64Vec3::raw_xyz( 4294967296, 4294967296, 4294967296)), - Vert(Planar64Vec3::raw_xyz( 4294967296,-4294967296, 4294967296)), - Vert(Planar64Vec3::raw_xyz(-4294967296, 4294967296,-4294967296)), - Vert(Planar64Vec3::raw_xyz(-4294967296, 4294967296, 4294967296)), - Vert(Planar64Vec3::raw_xyz(-4294967296,-4294967296, 4294967296)), - Vert(Planar64Vec3::raw_xyz(-4294967296,-4294967296,-4294967296)) + Vert(vec3::raw_xyz( 4294967296,-4294967296,-4294967296)), + Vert(vec3::raw_xyz( 4294967296, 4294967296,-4294967296)), + Vert(vec3::raw_xyz( 4294967296, 4294967296, 4294967296)), + Vert(vec3::raw_xyz( 4294967296,-4294967296, 4294967296)), + Vert(vec3::raw_xyz(-4294967296, 4294967296,-4294967296)), + Vert(vec3::raw_xyz(-4294967296, 4294967296, 4294967296)), + Vert(vec3::raw_xyz(-4294967296,-4294967296, 4294967296)), + Vert(vec3::raw_xyz(-4294967296,-4294967296,-4294967296)) ] }; let mesh_topology=PhysicsMeshTopology{ @@ -330,7 +329,7 @@ impl TryFrom<&model::Mesh> for PhysicsMesh{ for poly_vertices in polygon_group.polys(){ let submesh_face_id=SubmeshFaceId::new(submesh_faces.len() as u32); //one face per poly - let mut normal=Planar64Vec3::ZERO; + let mut normal=vec3::ZERO; let len=poly_vertices.len(); let face_edges=poly_vertices.into_iter().enumerate().map(|(i,vert_id)|{ let vert0_id=MeshVertId::new(mesh.unique_vertices[vert_id.get() as usize].pos.get() as u32); @@ -341,11 +340,11 @@ impl TryFrom<&model::Mesh> for PhysicsMesh{ //https://www.khronos.org/opengl/wiki/Calculating_a_Surface_Normal (Newell's Method) let v0=mesh.unique_pos[vert0_id.get() as usize]; let v1=mesh.unique_pos[vert1_id.get() as usize]; - normal+=Planar64Vec3::new( + normal+=Planar64Vec3::new([ (v0.y-v1.y)*(v0.z+v1.z), (v0.z-v1.z)*(v0.x+v1.x), (v0.x-v1.x)*(v0.y+v1.y), - ); + ]); //get/create edge and push face into it let (edge_ref_verts,is_sorted)=EdgeRefVerts::new(submesh_vert0_id,submesh_vert1_id); let (edge_ref_faces,edge_id)=edge_pool.push(edge_ref_verts); @@ -444,14 +443,14 @@ impl MeshQuery for PhysicsMes pub struct PhysicsMeshTransform{ pub vertex:integer::Planar64Affine3, - pub normal:integer::Planar64Mat3, - pub det:Planar64, + pub normal:integer::mat3::Matrix3>, + pub det:Fixed<3,96>, } impl PhysicsMeshTransform{ - pub const fn new(transform:integer::Planar64Affine3)->Self{ + pub fn new(transform:integer::Planar64Affine3)->Self{ Self{ - normal:transform.matrix3.inverse_times_det().transpose(), - det:transform.matrix3.determinant(), + normal:transform.matrix3.adjugate().transpose(), + det:transform.matrix3.det(), vertex:transform, } } @@ -471,30 +470,28 @@ impl TransformedMesh<'_>{ transform, } } - pub fn verts<'a>(&'a self)->impl Iterator+'a{ + pub fn verts<'a>(&'a self)->impl Iterator>>+'a{ self.view.data.verts.iter().map(|&Vert(pos)|self.transform.vertex.transform_point3(pos)) } fn farthest_vert(&self,dir:Planar64Vec3)->SubmeshVertId{ - let mut best_dot=Planar64::MIN; - let mut best_vert=SubmeshVertId(0); //this happens to be well-defined. there are no virtual virtices - for (i,vert_id) in self.view.topology.verts.iter().enumerate(){ - let p=self.transform.vertex.transform_point3(self.view.data.verts[vert_id.get() as usize].0); - let d=dir.dot(p); - if best_dot for TransformedMesh<'_>{ fn face_nd(&self,face_id:SubmeshFaceId)->(Planar64Vec3,Planar64){ let (n,d)=self.view.face_nd(face_id); let transformed_n=self.transform.normal*n; - let transformed_d=d+transformed_n.dot(self.transform.vertex.translation)/self.transform.det; - (transformed_n/self.transform.det,transformed_d) + let transformed_d=d+transformed_n.dot(self.transform.vertex.translation); + (transformed_n,transformed_d) } fn vert(&self,vert_id:SubmeshVertId)->Planar64Vec3{ self.transform.vertex.transform_point3(self.view.vert(vert_id)) @@ -600,7 +597,7 @@ impl MinkowskiMesh<'_>{ fn farthest_vert(&self,dir:Planar64Vec3)->MinkowskiVert{ MinkowskiVert::VertVert(self.mesh0.farthest_vert(dir),self.mesh1.farthest_vert(-dir)) } - fn next_transition_vert(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Planar64,infinity_dir:Planar64Vec3,point:Planar64Vec3)->Transition{ + fn next_transition_vert(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Fixed<2,64>,infinity_dir:Planar64Vec3,point:Planar64Vec3)->Transition{ let mut best_transition=Transition::Done; for &directed_edge_id in self.vert_edges(vert_id).iter(){ let edge_n=self.directed_edge_n(directed_edge_id); @@ -610,7 +607,7 @@ impl MinkowskiMesh<'_>{ let test_vert_id=edge_verts[directed_edge_id.parity() as usize]; //test if it's closer let diff=point-self.vert(test_vert_id); - if zeroes::zeroes1(edge_n.dot(diff),edge_n.dot(infinity_dir)).len()==0{ + if edge_n.dot(infinity_dir).is_zero(){ let distance_squared=diff.dot(diff); if distance_squared<*best_distance_squared{ best_transition=Transition::Vert(test_vert_id); @@ -620,21 +617,21 @@ impl MinkowskiMesh<'_>{ } best_transition } - fn final_ev(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Planar64,infinity_dir:Planar64Vec3,point:Planar64Vec3)->EV{ + fn final_ev(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Fixed<2,64>,infinity_dir:Planar64Vec3,point:Planar64Vec3)->EV{ let mut best_transition=EV::Vert(vert_id); let diff=point-self.vert(vert_id); for &directed_edge_id in self.vert_edges(vert_id).iter(){ let edge_n=self.directed_edge_n(directed_edge_id); //is boundary uncrossable by a crawl from infinity //check if time of collision is outside Time::MIN..Time::MAX - let d=edge_n.dot(diff); - if zeroes::zeroes1(d,edge_n.dot(infinity_dir)).len()==0{ + if edge_n.dot(infinity_dir).is_zero(){ + let d=edge_n.dot(diff); //test the edge let edge_nn=edge_n.dot(edge_n); - if Planar64::ZERO<=d&&d<=edge_nn{ + if !d.is_negative()&&d<=edge_nn{ let distance_squared={ let c=diff.cross(edge_n); - c.dot(c)/edge_nn + (c.dot(c)/edge_nn).divide().fix_2() }; if distance_squared<=*best_distance_squared{ best_transition=EV::Edge(directed_edge_id.as_undirected()); @@ -680,7 +677,7 @@ impl MinkowskiMesh<'_>{ let boundary_d=boundary_n.dot(delta_pos); //check if time of collision is outside Time::MIN..Time::MAX //infinity_dir can always be treated as a velocity - if (boundary_d)<=Planar64::ZERO&&zeroes::zeroes1(boundary_d,boundary_n.dot(infinity_dir)*2).len()==0{ + if !boundary_d.is_positive()&&boundary_n.dot(infinity_dir).is_zero(){ //both faces cannot pass this condition, return early if one does. return FEV::::Face(face_id); } @@ -694,7 +691,7 @@ impl MinkowskiMesh<'_>{ let infinity_fev=self.infinity_fev(-dir,infinity_body.position); //a line is simpler to solve than a parabola infinity_body.velocity=dir; - infinity_body.acceleration=Planar64Vec3::ZERO; + infinity_body.acceleration=vec3::ZERO; //crawl in from negative infinity along a tangent line to get the closest fev match crate::face_crawler::crawl_fev(infinity_fev,self,&infinity_body,integer::Time::MIN,infinity_body.time){ crate::face_crawler::CrawlResult::Miss(fev)=>Some(fev), @@ -740,9 +737,9 @@ impl MinkowskiMesh<'_>{ let verts=self.edge_verts(directed_edge_id.as_undirected()); let d=n.dot(self.vert(verts[0])+self.vert(verts[1])); //WARNING! d outside of *2 - for t in zeroes::zeroes2((n.dot(relative_body.position))*2-d,n.dot(relative_body.velocity)*2,n.dot(relative_body.acceleration)){ + for t in Fixed::<3,96>::zeroes2((n.dot(relative_body.position))*2-d,n.dot(relative_body.velocity)*2,n.dot(relative_body.acceleration)){ let t=relative_body.time+integer::Time::from(t); - if relative_body.time{ } } pub fn is_point_in_mesh(&self,point:Planar64Vec3)->bool{ - let infinity_body=crate::physics::Body::new(point,Planar64Vec3::Y,Planar64Vec3::ZERO,integer::Time::ZERO); + let infinity_body=crate::physics::Body::new(point,vec3::Y,vec3::ZERO,integer::Time::ZERO); //movement must escape the mesh forwards and backwards in time, //otherwise the point is not inside the mesh self.infinity_in(infinity_body) @@ -950,7 +947,7 @@ fn is_empty_volume(normals:Vec)->bool{ for i in 0..len-1{ for j in i+1..len{ let n=normals[i].cross(normals[j]); - let mut d_comp=None; + let mut d_comp:Option>=None; for k in 0..len{ if k!=i&&k!=j{ let d=n.dot(normals[k]); @@ -970,8 +967,8 @@ fn is_empty_volume(normals:Vec)->bool{ #[test] fn test_is_empty_volume(){ - assert!(!is_empty_volume([Planar64Vec3::X,Planar64Vec3::Y,Planar64Vec3::Z].to_vec())); - assert!(is_empty_volume([Planar64Vec3::X,Planar64Vec3::Y,Planar64Vec3::Z,Planar64Vec3::NEG_X].to_vec())); + assert!(!is_empty_volume([vec3::X,vec3::Y,vec3::Z].to_vec())); + assert!(is_empty_volume([vec3::X,vec3::Y,vec3::Z,vec3::NEG_X].to_vec())); } #[test]