forked from StrafesNET/strafe-client
wip
This commit is contained in:
parent
bbc9a89ad7
commit
33c9fb0399
@ -1,7 +1,6 @@
|
|||||||
use crate::physics::Body;
|
use crate::physics::Body;
|
||||||
use crate::model_physics::{FEV,MeshQuery,DirectedEdge};
|
use crate::model_physics::{FEV,MeshQuery,DirectedEdge};
|
||||||
use strafesnet_common::integer::{Time,Planar64};
|
use strafesnet_common::integer::{Time,Fixed,Planar64};
|
||||||
use strafesnet_common::zeroes::zeroes2;
|
|
||||||
|
|
||||||
enum Transition<F,E:DirectedEdge,V>{
|
enum Transition<F,E:DirectedEdge,V>{
|
||||||
Miss,
|
Miss,
|
||||||
@ -22,9 +21,9 @@ enum Transition<F,E:DirectedEdge,V>{
|
|||||||
let (n,d)=mesh.face_nd(face_id);
|
let (n,d)=mesh.face_nd(face_id);
|
||||||
//TODO: use higher precision d value?
|
//TODO: use higher precision d value?
|
||||||
//use the mesh transform translation instead of baking it into the 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);
|
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)).is_negative(){
|
||||||
best_time=t;
|
best_time=t;
|
||||||
best_transtition=Transition::Hit(face_id,t);
|
best_transtition=Transition::Hit(face_id,t);
|
||||||
break;
|
break;
|
||||||
@ -38,7 +37,7 @@ enum Transition<F,E:DirectedEdge,V>{
|
|||||||
//WARNING: d is moved out of the *2 block because of adding two vertices!
|
//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)){
|
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);
|
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)).is_negative(){
|
||||||
best_time=t;
|
best_time=t;
|
||||||
best_transtition=Transition::Next(FEV::<F,E,V>::Edge(directed_edge_id.as_undirected()),t);
|
best_transtition=Transition::Next(FEV::<F,E,V>::Edge(directed_edge_id.as_undirected()),t);
|
||||||
break;
|
break;
|
||||||
@ -59,7 +58,7 @@ enum Transition<F,E:DirectedEdge,V>{
|
|||||||
//WARNING yada yada d *2
|
//WARNING yada yada d *2
|
||||||
for t in zeroes2(n.dot(delta_pos),n.dot(body.velocity)*2,n.dot(body.acceleration)){
|
for t in zeroes2(n.dot(delta_pos),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)).is_negative(){
|
||||||
best_time=t;
|
best_time=t;
|
||||||
best_transtition=Transition::Next(FEV::<F,E,V>::Face(edge_face_id),t);
|
best_transtition=Transition::Next(FEV::<F,E,V>::Face(edge_face_id),t);
|
||||||
break;
|
break;
|
||||||
@ -72,7 +71,7 @@ enum Transition<F,E:DirectedEdge,V>{
|
|||||||
let n=edge_n*(1-2*(i as i64));
|
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)){
|
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);
|
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)).is_negative(){
|
||||||
best_time=t;
|
best_time=t;
|
||||||
best_transtition=Transition::Next(FEV::<F,E,V>::Vert(vert_id),t);
|
best_transtition=Transition::Next(FEV::<F,E,V>::Vert(vert_id),t);
|
||||||
break;
|
break;
|
||||||
@ -88,7 +87,7 @@ enum Transition<F,E:DirectedEdge,V>{
|
|||||||
let n=-mesh.directed_edge_n(directed_edge_id);
|
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)){
|
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);
|
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)).is_negative(){
|
||||||
best_time=t;
|
best_time=t;
|
||||||
best_transtition=Transition::Next(FEV::<F,E,V>::Edge(directed_edge_id.as_undirected()),t);
|
best_transtition=Transition::Next(FEV::<F,E,V>::Edge(directed_edge_id.as_undirected()),t);
|
||||||
break;
|
break;
|
||||||
|
@ -1,8 +1,7 @@
|
|||||||
use std::borrow::{Borrow,Cow};
|
use std::borrow::{Borrow,Cow};
|
||||||
use std::collections::{HashSet,HashMap};
|
use std::collections::{HashSet,HashMap};
|
||||||
use strafesnet_common::model::{self,MeshId,PolygonIter};
|
use strafesnet_common::model::{self,MeshId,PolygonIter};
|
||||||
use strafesnet_common::zeroes;
|
use strafesnet_common::integer::{self,vec3,Fixed,Planar64,Planar64Vec3};
|
||||||
use strafesnet_common::integer::{self,Planar64,Planar64Vec3};
|
|
||||||
|
|
||||||
pub trait UndirectedEdge{
|
pub trait UndirectedEdge{
|
||||||
type DirectedEdge:Copy+DirectedEdge;
|
type DirectedEdge:Copy+DirectedEdge;
|
||||||
@ -137,22 +136,22 @@ impl PhysicsMesh{
|
|||||||
//go go gadget debug print mesh
|
//go go gadget debug print mesh
|
||||||
let data=PhysicsMeshData{
|
let data=PhysicsMeshData{
|
||||||
faces:vec![
|
faces:vec![
|
||||||
Face{normal:Planar64Vec3::raw_xyz( 4294967296, 0, 0),dot:Planar64::raw(4294967296)},
|
Face{normal:vec3::raw_xyz( 4294967296, 0, 0),dot:Planar64::raw(4294967296)},
|
||||||
Face{normal:Planar64Vec3::raw_xyz( 0, 4294967296, 0),dot:Planar64::raw(4294967296)},
|
Face{normal:vec3::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( 0, 0, 4294967296),dot:Planar64::raw(4294967296)},
|
||||||
Face{normal:Planar64Vec3::raw_xyz(-4294967296, 0, 0),dot:Planar64::raw(4294967296)},
|
Face{normal:vec3::raw_xyz(-4294967296, 0, 0),dot:Planar64::raw(4294967296)},
|
||||||
Face{normal:Planar64Vec3::raw_xyz( 0,-4294967296, 0),dot:Planar64::raw(4294967296)},
|
Face{normal:vec3::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( 0, 0,-4294967296),dot:Planar64::raw(4294967296)}
|
||||||
],
|
],
|
||||||
verts:vec![
|
verts:vec![
|
||||||
Vert(Planar64Vec3::raw_xyz( 4294967296,-4294967296,-4294967296)),
|
Vert(vec3::raw_xyz( 4294967296,-4294967296,-4294967296)),
|
||||||
Vert(Planar64Vec3::raw_xyz( 4294967296, 4294967296,-4294967296)),
|
Vert(vec3::raw_xyz( 4294967296, 4294967296,-4294967296)),
|
||||||
Vert(Planar64Vec3::raw_xyz( 4294967296, 4294967296, 4294967296)),
|
Vert(vec3::raw_xyz( 4294967296, 4294967296, 4294967296)),
|
||||||
Vert(Planar64Vec3::raw_xyz( 4294967296,-4294967296, 4294967296)),
|
Vert(vec3::raw_xyz( 4294967296,-4294967296, 4294967296)),
|
||||||
Vert(Planar64Vec3::raw_xyz(-4294967296, 4294967296,-4294967296)),
|
Vert(vec3::raw_xyz(-4294967296, 4294967296,-4294967296)),
|
||||||
Vert(Planar64Vec3::raw_xyz(-4294967296, 4294967296, 4294967296)),
|
Vert(vec3::raw_xyz(-4294967296, 4294967296, 4294967296)),
|
||||||
Vert(Planar64Vec3::raw_xyz(-4294967296,-4294967296, 4294967296)),
|
Vert(vec3::raw_xyz(-4294967296,-4294967296, 4294967296)),
|
||||||
Vert(Planar64Vec3::raw_xyz(-4294967296,-4294967296,-4294967296))
|
Vert(vec3::raw_xyz(-4294967296,-4294967296,-4294967296))
|
||||||
]
|
]
|
||||||
};
|
};
|
||||||
let mesh_topology=PhysicsMeshTopology{
|
let mesh_topology=PhysicsMeshTopology{
|
||||||
@ -330,7 +329,7 @@ impl TryFrom<&model::Mesh> for PhysicsMesh{
|
|||||||
for poly_vertices in polygon_group.polys(){
|
for poly_vertices in polygon_group.polys(){
|
||||||
let submesh_face_id=SubmeshFaceId::new(submesh_faces.len() as u32);
|
let submesh_face_id=SubmeshFaceId::new(submesh_faces.len() as u32);
|
||||||
//one face per poly
|
//one face per poly
|
||||||
let mut normal=Planar64Vec3::ZERO;
|
let mut normal=vec3::ZERO;
|
||||||
let len=poly_vertices.len();
|
let len=poly_vertices.len();
|
||||||
let face_edges=poly_vertices.into_iter().enumerate().map(|(i,vert_id)|{
|
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);
|
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)
|
//https://www.khronos.org/opengl/wiki/Calculating_a_Surface_Normal (Newell's Method)
|
||||||
let v0=mesh.unique_pos[vert0_id.get() as usize];
|
let v0=mesh.unique_pos[vert0_id.get() as usize];
|
||||||
let v1=mesh.unique_pos[vert1_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.y-v1.y)*(v0.z+v1.z),
|
||||||
(v0.z-v1.z)*(v0.x+v1.x),
|
(v0.z-v1.z)*(v0.x+v1.x),
|
||||||
(v0.x-v1.x)*(v0.y+v1.y),
|
(v0.x-v1.x)*(v0.y+v1.y),
|
||||||
);
|
]);
|
||||||
//get/create edge and push face into it
|
//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_verts,is_sorted)=EdgeRefVerts::new(submesh_vert0_id,submesh_vert1_id);
|
||||||
let (edge_ref_faces,edge_id)=edge_pool.push(edge_ref_verts);
|
let (edge_ref_faces,edge_id)=edge_pool.push(edge_ref_verts);
|
||||||
@ -444,14 +443,14 @@ impl MeshQuery<SubmeshFaceId,SubmeshDirectedEdgeId,SubmeshVertId> for PhysicsMes
|
|||||||
|
|
||||||
pub struct PhysicsMeshTransform{
|
pub struct PhysicsMeshTransform{
|
||||||
pub vertex:integer::Planar64Affine3,
|
pub vertex:integer::Planar64Affine3,
|
||||||
pub normal:integer::Planar64Mat3,
|
pub normal:integer::mat3::Matrix3<Fixed<2,64>>,
|
||||||
pub det:Planar64,
|
pub det:Fixed<3,96>,
|
||||||
}
|
}
|
||||||
impl PhysicsMeshTransform{
|
impl PhysicsMeshTransform{
|
||||||
pub const fn new(transform:integer::Planar64Affine3)->Self{
|
pub fn new(transform:integer::Planar64Affine3)->Self{
|
||||||
Self{
|
Self{
|
||||||
normal:transform.matrix3.inverse_times_det().transpose(),
|
normal:transform.matrix3.adjugate().transpose(),
|
||||||
det:transform.matrix3.determinant(),
|
det:transform.matrix3.det(),
|
||||||
vertex:transform,
|
vertex:transform,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -471,30 +470,28 @@ impl TransformedMesh<'_>{
|
|||||||
transform,
|
transform,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
pub fn verts<'a>(&'a self)->impl Iterator<Item=Planar64Vec3>+'a{
|
pub fn verts<'a>(&'a self)->impl Iterator<Item=vec3::Vector3<Fixed<2,64>>>+'a{
|
||||||
self.view.data.verts.iter().map(|&Vert(pos)|self.transform.vertex.transform_point3(pos))
|
self.view.data.verts.iter().map(|&Vert(pos)|self.transform.vertex.transform_point3(pos))
|
||||||
}
|
}
|
||||||
fn farthest_vert(&self,dir:Planar64Vec3)->SubmeshVertId{
|
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
|
//this happens to be well-defined. there are no virtual virtices
|
||||||
for (i,vert_id) in self.view.topology.verts.iter().enumerate(){
|
SubmeshVertId::new(
|
||||||
let p=self.transform.vertex.transform_point3(self.view.data.verts[vert_id.get() as usize].0);
|
self.view.topology.verts.iter()
|
||||||
let d=dir.dot(p);
|
.enumerate()
|
||||||
if best_dot<d{
|
.min_by_key(|(_,vert_id)|
|
||||||
best_dot=d;
|
dir.dot(self.transform.vertex.transform_point3(self.view.data.verts[vert_id.get() as usize].0))
|
||||||
best_vert=SubmeshVertId::new(i as u32);
|
)
|
||||||
}
|
//assume there is more than zero vertices.
|
||||||
}
|
.unwrap().0 as u32
|
||||||
best_vert
|
)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
impl MeshQuery<SubmeshFaceId,SubmeshDirectedEdgeId,SubmeshVertId> for TransformedMesh<'_>{
|
impl MeshQuery<SubmeshFaceId,SubmeshDirectedEdgeId,SubmeshVertId> for TransformedMesh<'_>{
|
||||||
fn face_nd(&self,face_id:SubmeshFaceId)->(Planar64Vec3,Planar64){
|
fn face_nd(&self,face_id:SubmeshFaceId)->(Planar64Vec3,Planar64){
|
||||||
let (n,d)=self.view.face_nd(face_id);
|
let (n,d)=self.view.face_nd(face_id);
|
||||||
let transformed_n=self.transform.normal*n;
|
let transformed_n=self.transform.normal*n;
|
||||||
let transformed_d=d+transformed_n.dot(self.transform.vertex.translation)/self.transform.det;
|
let transformed_d=d+transformed_n.dot(self.transform.vertex.translation);
|
||||||
(transformed_n/self.transform.det,transformed_d)
|
(transformed_n,transformed_d)
|
||||||
}
|
}
|
||||||
fn vert(&self,vert_id:SubmeshVertId)->Planar64Vec3{
|
fn vert(&self,vert_id:SubmeshVertId)->Planar64Vec3{
|
||||||
self.transform.vertex.transform_point3(self.view.vert(vert_id))
|
self.transform.vertex.transform_point3(self.view.vert(vert_id))
|
||||||
@ -600,7 +597,7 @@ 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 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;
|
let mut best_transition=Transition::Done;
|
||||||
for &directed_edge_id in self.vert_edges(vert_id).iter(){
|
for &directed_edge_id in self.vert_edges(vert_id).iter(){
|
||||||
let edge_n=self.directed_edge_n(directed_edge_id);
|
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];
|
let test_vert_id=edge_verts[directed_edge_id.parity() as usize];
|
||||||
//test if it's closer
|
//test if it's closer
|
||||||
let diff=point-self.vert(test_vert_id);
|
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);
|
let distance_squared=diff.dot(diff);
|
||||||
if distance_squared<*best_distance_squared{
|
if distance_squared<*best_distance_squared{
|
||||||
best_transition=Transition::Vert(test_vert_id);
|
best_transition=Transition::Vert(test_vert_id);
|
||||||
@ -620,21 +617,21 @@ impl MinkowskiMesh<'_>{
|
|||||||
}
|
}
|
||||||
best_transition
|
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 mut best_transition=EV::Vert(vert_id);
|
||||||
let diff=point-self.vert(vert_id);
|
let diff=point-self.vert(vert_id);
|
||||||
for &directed_edge_id in self.vert_edges(vert_id).iter(){
|
for &directed_edge_id in self.vert_edges(vert_id).iter(){
|
||||||
let edge_n=self.directed_edge_n(directed_edge_id);
|
let edge_n=self.directed_edge_n(directed_edge_id);
|
||||||
//is boundary uncrossable by a crawl from infinity
|
//is boundary uncrossable by a crawl from infinity
|
||||||
//check if time of collision is outside Time::MIN..Time::MAX
|
//check if time of collision is outside Time::MIN..Time::MAX
|
||||||
let d=edge_n.dot(diff);
|
if edge_n.dot(infinity_dir).is_zero(){
|
||||||
if zeroes::zeroes1(d,edge_n.dot(infinity_dir)).len()==0{
|
let d=edge_n.dot(diff);
|
||||||
//test the edge
|
//test the edge
|
||||||
let edge_nn=edge_n.dot(edge_n);
|
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 distance_squared={
|
||||||
let c=diff.cross(edge_n);
|
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{
|
if distance_squared<=*best_distance_squared{
|
||||||
best_transition=EV::Edge(directed_edge_id.as_undirected());
|
best_transition=EV::Edge(directed_edge_id.as_undirected());
|
||||||
@ -680,7 +677,7 @@ impl MinkowskiMesh<'_>{
|
|||||||
let boundary_d=boundary_n.dot(delta_pos);
|
let boundary_d=boundary_n.dot(delta_pos);
|
||||||
//check if time of collision is outside Time::MIN..Time::MAX
|
//check if time of collision is outside Time::MIN..Time::MAX
|
||||||
//infinity_dir can always be treated as a velocity
|
//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.
|
//both faces cannot pass this condition, return early if one does.
|
||||||
return FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>::Face(face_id);
|
return FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>::Face(face_id);
|
||||||
}
|
}
|
||||||
@ -694,7 +691,7 @@ impl MinkowskiMesh<'_>{
|
|||||||
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
|
||||||
infinity_body.velocity=dir;
|
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
|
//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){
|
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),
|
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 verts=self.edge_verts(directed_edge_id.as_undirected());
|
||||||
let d=n.dot(self.vert(verts[0])+self.vert(verts[1]));
|
let d=n.dot(self.vert(verts[0])+self.vert(verts[1]));
|
||||||
//WARNING! d outside of *2
|
//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);
|
let t=relative_body.time+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)).is_negative(){
|
||||||
best_time=t;
|
best_time=t;
|
||||||
best_edge=Some(directed_edge_id);
|
best_edge=Some(directed_edge_id);
|
||||||
break;
|
break;
|
||||||
@ -759,7 +756,7 @@ impl MinkowskiMesh<'_>{
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
pub fn is_point_in_mesh(&self,point:Planar64Vec3)->bool{
|
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,
|
//movement must escape the mesh forwards and backwards in time,
|
||||||
//otherwise the point is not inside the mesh
|
//otherwise the point is not inside the mesh
|
||||||
self.infinity_in(infinity_body)
|
self.infinity_in(infinity_body)
|
||||||
@ -950,7 +947,7 @@ fn is_empty_volume(normals:Vec<Planar64Vec3>)->bool{
|
|||||||
for i in 0..len-1{
|
for i in 0..len-1{
|
||||||
for j in i+1..len{
|
for j in i+1..len{
|
||||||
let n=normals[i].cross(normals[j]);
|
let n=normals[i].cross(normals[j]);
|
||||||
let mut d_comp=None;
|
let mut d_comp:Option<Fixed<3,96>>=None;
|
||||||
for k in 0..len{
|
for k in 0..len{
|
||||||
if k!=i&&k!=j{
|
if k!=i&&k!=j{
|
||||||
let d=n.dot(normals[k]);
|
let d=n.dot(normals[k]);
|
||||||
@ -970,8 +967,8 @@ fn is_empty_volume(normals:Vec<Planar64Vec3>)->bool{
|
|||||||
|
|
||||||
#[test]
|
#[test]
|
||||||
fn test_is_empty_volume(){
|
fn test_is_empty_volume(){
|
||||||
assert!(!is_empty_volume([Planar64Vec3::X,Planar64Vec3::Y,Planar64Vec3::Z].to_vec()));
|
assert!(!is_empty_volume([vec3::X,vec3::Y,vec3::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,vec3::NEG_X].to_vec()));
|
||||||
}
|
}
|
||||||
|
|
||||||
#[test]
|
#[test]
|
||||||
|
Loading…
Reference in New Issue
Block a user