Compare commits

...

29 Commits

Author SHA1 Message Date
679f8ca89b ??? 2026-01-30 09:22:47 -08:00
0b9c871a21 wip specialized fixed ratio 2026-01-30 09:04:36 -08:00
a333d75bbd delete ratio_ops 2026-01-30 09:04:25 -08:00
534f2a2141 fixed: silence lint 2026-01-30 07:52:52 -08:00
79ea88fc74 fixed: remove pub(crate) field visibility 2026-01-30 07:52:42 -08:00
3fd507be94 noclip 2026-01-30 07:33:04 -08:00
0fbe37e483 Rewrite MeshQuery (#33)
Splits the MeshQuery trait into MeshQuery and MeshTopology and cleans up much of the physics traits.  A notable optimization is using a closure for iterating variable length topological lists.  Intermediate allocations are avoided in this way.

Reviewed-on: #33
Co-authored-by: Rhys Lloyd <krakow20@gmail.com>
Co-committed-by: Rhys Lloyd <krakow20@gmail.com>
2026-01-29 18:05:54 +00:00
638c2b4329 shuffle code around 2026-01-29 09:44:21 -08:00
317e1d57c7 move minkowski into module 2026-01-29 09:40:46 -08:00
562e46a87e defer body update to actual strafe tick 2026-01-28 09:57:33 -08:00
3c13d5f8ec untab 2026-01-28 08:40:14 -08:00
1f0f78f9d8 tweak Trajectory code 2026-01-28 08:39:28 -08:00
a90cb53a20 delete awful functions 2026-01-28 07:28:22 -08:00
170e2b9bce return used contacts from push_solve 2026-01-27 09:28:26 -08:00
3e0fc54852 Delete Body.acceleration Field (#30)
Acceleration is not a persistent part of the PhysicsState, and is only used for intermediate calculations along a trajectory.

Fixes several bugs:
- Walk decelerate clip into wall
- Walk accelerate clip into wall
- Fall while flying

Reviewed-on: #30
Co-authored-by: Rhys Lloyd <krakow20@gmail.com>
Co-committed-by: Rhys Lloyd <krakow20@gmail.com>
2026-01-27 17:08:08 +00:00
acea52646a untab 2026-01-27 07:58:18 -08:00
7220506fd5 plumb sprint 2026-01-27 07:46:17 -08:00
8f94234ddc change DirectedEdge signature 2026-01-27 07:38:45 -08:00
36143b8b69 change UndirectedEdge signature 2026-01-27 07:35:43 -08:00
3893b2f44f work around reset bug 2026-01-26 09:19:31 -08:00
d62ff68baa fix comments 2026-01-26 09:06:40 -08:00
2331bef281 pretty print time 2026-01-22 09:43:35 -08:00
31b52f7c34 Conditionally Advance Body in atomic_internal_instruction StrafeTick (#14)
Closes #13, but exposes the underlying issue at all times.

Reviewed-on: #14
Co-authored-by: Quaternions <krakow20@gmail.com>
Co-committed-by: Quaternions <krakow20@gmail.com>
2026-01-22 17:00:52 +00:00
b12c495a33 it: bug 3 2026-01-22 08:57:35 -08:00
8329eadb18 use unbounded range in physics tests 2026-01-22 08:47:54 -08:00
4b2f93be66 fix algorithm setup start position 2026-01-22 08:41:54 -08:00
469ab48156 allow unbounded range 2026-01-22 08:41:35 -08:00
c2650adf54 md: simplify reduce 2026-01-21 10:32:19 -08:00
cdafbb4077 Implement MinimumDifference Algorithm (#25)
Completely replace the janky closest fev crawl from infinity algorithm with a dedicated purpose-built algorithm.  Finding the closest point on a MinkowskiMesh is equivalent to finding the closest point between two meshes.

Reviewed-on: #25
Co-authored-by: Rhys Lloyd <krakow20@gmail.com>
Co-committed-by: Rhys Lloyd <krakow20@gmail.com>
2026-01-21 17:31:52 +00:00
27 changed files with 1893 additions and 1516 deletions

View File

@@ -909,7 +909,7 @@ impl GraphicsState{
// update rotation
let camera_uniforms=self.camera.to_uniform_data(
frame_state.body.extrapolated_position(frame_state.time).map(Into::<f32>::into).to_array().into(),
frame_state.trajectory.extrapolated_position(frame_state.time).map(Into::<f32>::into).to_array().into(),
frame_state.camera.simulate_move_angles(glam::IVec2::ZERO)
);
self.staging_belt

View File

@@ -2,12 +2,18 @@ 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!
pub position:Planar64Vec3,
pub velocity:Planar64Vec3,
pub time:Time<T>,
}
impl<T> std::ops::Neg for Body<T>{
#[derive(Clone,Copy,Debug,Hash)]
pub struct Trajectory<T>{
pub position:Planar64Vec3,
pub velocity:Planar64Vec3,
pub acceleration:Planar64Vec3,
pub time:Time<T>,
}
impl<T> std::ops::Neg for Trajectory<T>{
type Output=Self;
fn neg(self)->Self::Output{
Self{
@@ -18,10 +24,10 @@ impl<T> std::ops::Neg for Body<T>{
}
}
}
impl<T:Copy> std::ops::Neg for &Body<T>{
type Output=Body<T>;
impl<T:Copy> std::ops::Neg for &Trajectory<T>{
type Output=Trajectory<T>;
fn neg(self)->Self::Output{
Body{
Trajectory{
position:self.position,
velocity:-self.velocity,
acceleration:self.acceleration,
@@ -32,6 +38,32 @@ impl<T:Copy> std::ops::Neg for &Body<T>{
impl<T> Body<T>
where Time<T>:Copy,
{
pub const ZERO:Self=Self::new(vec3::zero(),vec3::zero(),Time::ZERO);
pub const fn new(position:Planar64Vec3,velocity:Planar64Vec3,time:Time<T>)->Self{
Self{
position,
velocity,
time,
}
}
pub const fn with_acceleration(self,acceleration:Planar64Vec3)->Trajectory<T>{
let Body{
position,
velocity,
time,
}=self;
Trajectory{
position,
velocity,
acceleration,
time,
}
}
}
impl<T> Trajectory<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{
@@ -42,13 +74,14 @@ impl<T> Body<T>
time,
}
}
pub const fn relative_to<'a>(&'a self,body0:&'a Body<T>)->VirtualBody<'a,T>{
pub fn relative_to(&self,trj0:&Self,time:Time<T>)->Self{
//(p0,v0,a0,t0)
//(p1,v1,a1,t1)
VirtualBody{
body0,
body1:self,
}
Trajectory::new(
self.extrapolated_position(time)-trj0.extrapolated_position(time),
self.extrapolated_velocity(time)-trj0.extrapolated_velocity(time),
self.acceleration-trj0.acceleration,
time)
}
pub fn extrapolated_position(&self,time:Time<T>)->Planar64Vec3{
let dt=time-self.time;
@@ -60,10 +93,12 @@ impl<T> Body<T>
let dt=time-self.time;
self.velocity+(self.acceleration*dt).map(|elem|elem.divide().clamp_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_body(&self,time:Time<T>)->Body<T>{
Body::new(
self.extrapolated_position(time),
self.extrapolated_velocity(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
@@ -101,10 +136,12 @@ impl<T> Body<T>
// a*dt + v
self.acceleration.map(|elem|(dt*elem).divide().clamp())+self.velocity
}
pub fn advance_time_ratio_dt(&mut self,dt:crate::model::GigaTime){
self.position=self.extrapolated_position_ratio_dt(dt);
self.velocity=self.extrapolated_velocity_ratio_dt(dt);
self.time+=dt.into();
pub fn extrapolated_body_ratio_dt(&self,dt:crate::model::GigaTime)->Body<T>{
Body::new(
self.extrapolated_position_ratio_dt(dt),
self.extrapolated_velocity_ratio_dt(dt),
self.time+dt.into(),
)
}
pub fn infinity_dir(&self)->Option<Planar64Vec3>{
if self.velocity==vec3::zero(){
@@ -144,28 +181,12 @@ impl<T> Body<T>
}
impl<T> std::fmt::Display for Body<T>{
fn fmt(&self,f:&mut std::fmt::Formatter<'_>)->std::fmt::Result{
write!(f,"p({}) v({}) t({})",self.position,self.velocity,self.time)
}
}
impl<T> std::fmt::Display for Trajectory<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 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)
}
}

View File

@@ -1,32 +1,27 @@
use crate::model::{into_giga_time,GigaTime,FEV,MeshQuery,DirectedEdge};
use strafesnet_common::integer::{Fixed,Ratio,vec3::Vector3};
use crate::physics::{Time,Body};
use crate::model::{into_giga_time,GigaTime};
use strafesnet_common::integer::{Fixed,Ratio,vec3::Vector3,Planar64Vec3};
use crate::physics::{Time,Trajectory};
use crate::mesh_query::{FEV,DirectedEdge,MeshQuery,MeshTopology};
use core::ops::Bound;
enum Transition<M:MeshQuery>{
enum Transition<M:MeshTopology>{
Miss,
Next(FEV<M>,GigaTime),
Hit(M::Face,GigaTime),
}
pub enum CrawlResult<M:MeshQuery>{
pub enum CrawlResult<M:MeshTopology>{
Miss(FEV<M>),
Hit(M::Face,GigaTime),
}
impl<M:MeshQuery> CrawlResult<M>{
impl<M:MeshTopology> CrawlResult<M>{
pub fn hit(self)->Option<(M::Face,GigaTime)>{
match self{
CrawlResult::Miss(_)=>None,
CrawlResult::Hit(face,time)=>Some((face,time)),
}
}
pub fn miss(self)->Option<FEV<M>>{
match self{
CrawlResult::Miss(fev)=>Some(fev),
CrawlResult::Hit(_,_)=>None,
}
}
}
// TODO: move predict_collision_face_out algorithm in here or something
@@ -70,17 +65,18 @@ where
}
}
impl<F:Copy,M:MeshQuery<Normal=Vector3<F>,Offset=Fixed<4,128>>> FEV<M>
impl<F:Copy,M:MeshQuery<Normal=Vector3<F>,Offset=Fixed<4,128>,Position=Planar64Vec3,Direction=Planar64Vec3>> FEV<M>
where
// This is hardcoded for MinkowskiMesh lol
M::Face:Copy,
M::Edge:Copy,
M::DirectedEdge: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::Offset:core::ops::Sub<<F as std::ops::Mul<Fixed<1,32>>>::Output>,
{
fn next_transition(&self,mesh:&M,body:&Body,lower_bound:Bound<GigaTime>,mut upper_bound:Bound<GigaTime>)->Transition<M>{
fn next_transition(&self,mesh:&M,trajectory:&Trajectory,lower_bound:Bound<GigaTime>,mut upper_bound:Bound<GigaTime>)->Transition<M>{
//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.
let mut best_transition=Transition::Miss;
@@ -92,29 +88,29 @@ impl<F:Copy,M:MeshQuery<Normal=Vector3<F>,Offset=Fixed<4,128>>> FEV<M>
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 dt in Fixed::<4,128>::zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
for dt in Fixed::<4,128>::zeroes2((n.dot(trajectory.position)-d)*2,n.dot(trajectory.velocity)*2,n.dot(trajectory.acceleration)){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(trajectory.extrapolated_velocity_ratio_dt(dt)).is_negative(){
upper_bound=Bound::Included(dt);
best_transition=Transition::Hit(face_id,dt);
break;
}
}
//test each edge collision time, ignoring roots with zero or conflicting derivative
for &directed_edge_id in mesh.face_edges(face_id).as_ref(){
mesh.for_each_face_edge(face_id,|directed_edge_id|{
let edge_n=mesh.directed_edge_n(directed_edge_id);
let n=n.cross(edge_n);
let &[v0,v1]=mesh.edge_verts(directed_edge_id.as_undirected()).as_ref();
//WARNING: d is moved out of the *2 block because of adding two vertices!
//WARNING: precision is swept under the rug!
//wrap for speed
for dt in Fixed::<4,128>::zeroes2(n.dot(body.position*2-(mesh.vert(v0)+mesh.vert(v1))).wrap_4(),n.dot(body.velocity).wrap_4()*2,n.dot(body.acceleration).wrap_4()){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
for dt in Fixed::<4,128>::zeroes2(n.dot(trajectory.position*2-(mesh.vert(v0)+mesh.vert(v1))).wrap_4(),n.dot(trajectory.velocity).wrap_4()*2,n.dot(trajectory.acceleration).wrap_4()){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(trajectory.extrapolated_velocity_ratio_dt(dt)).is_negative(){
upper_bound=Bound::Included(dt);
best_transition=Transition::Next(FEV::Edge(directed_edge_id.as_undirected()),dt);
break;
}
}
}
});
//if none:
},
&FEV::Edge(edge_id)=>{
@@ -123,15 +119,15 @@ impl<F:Copy,M:MeshQuery<Normal=Vector3<F>,Offset=Fixed<4,128>>> FEV<M>
let &[ev0,ev1]=edge_verts.as_ref();
let (v0,v1)=(mesh.vert(ev0),mesh.vert(ev1));
let edge_n=v1-v0;
let delta_pos=body.position*2-(v0+v1);
let delta_pos=trajectory.position*2-(v0+v1);
for (i,&edge_face_id) in mesh.edge_faces(edge_id).as_ref().iter().enumerate(){
let face_n=mesh.face_nd(edge_face_id).0;
//edge_n gets parity from the order of edge_faces
let n=face_n.cross(edge_n)*((i as i64)*2-1);
//WARNING yada yada d *2
//wrap for speed
for dt in Fixed::<4,128>::zeroes2(n.dot(delta_pos).wrap_4(),n.dot(body.velocity).wrap_4()*2,n.dot(body.acceleration).wrap_4()){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
for dt in Fixed::<4,128>::zeroes2(n.dot(delta_pos).wrap_4(),n.dot(trajectory.velocity).wrap_4()*2,n.dot(trajectory.acceleration).wrap_4()){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(trajectory.extrapolated_velocity_ratio_dt(dt)).is_negative(){
upper_bound=Bound::Included(dt);
best_transition=Transition::Next(FEV::Face(edge_face_id),dt);
break;
@@ -142,8 +138,8 @@ impl<F:Copy,M:MeshQuery<Normal=Vector3<F>,Offset=Fixed<4,128>>> FEV<M>
for (i,&vert_id) in edge_verts.as_ref().iter().enumerate(){
//vertex normal gets parity from vert index
let n=edge_n*(1-2*(i as i64));
for dt in Fixed::<2,64>::zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
for dt in Fixed::<2,64>::zeroes2((n.dot(trajectory.position-mesh.vert(vert_id)))*2,n.dot(trajectory.velocity)*2,n.dot(trajectory.acceleration)){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(trajectory.extrapolated_velocity_ratio_dt(dt)).is_negative(){
let dt=Ratio::new(dt.num.widen_4(),dt.den.widen_4());
upper_bound=Bound::Included(dt);
best_transition=Transition::Next(FEV::Vert(vert_id),dt);
@@ -155,28 +151,28 @@ impl<F:Copy,M:MeshQuery<Normal=Vector3<F>,Offset=Fixed<4,128>>> FEV<M>
},
&FEV::Vert(vert_id)=>{
//test each edge collision time, ignoring roots with zero or conflicting derivative
for &directed_edge_id in mesh.vert_edges(vert_id).as_ref(){
mesh.for_each_vert_edge(vert_id,|directed_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);
for dt in Fixed::<2,64>::zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
for dt in Fixed::<2,64>::zeroes2((n.dot(trajectory.position-mesh.vert(vert_id)))*2,n.dot(trajectory.velocity)*2,n.dot(trajectory.acceleration)){
if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(trajectory.extrapolated_velocity_ratio_dt(dt)).is_negative(){
let dt=Ratio::new(dt.num.widen_4(),dt.den.widen_4());
upper_bound=Bound::Included(dt);
best_transition=Transition::Next(FEV::Edge(directed_edge_id.as_undirected()),dt);
break;
}
}
}
});
//if none:
},
}
best_transition
}
pub fn crawl(mut self,mesh:&M,relative_body:&Body,lower_bound:Bound<&Time>,upper_bound:Bound<&Time>)->CrawlResult<M>{
let mut lower_bound=lower_bound.map(|&t|into_giga_time(t,relative_body.time));
let upper_bound=upper_bound.map(|&t|into_giga_time(t,relative_body.time));
pub fn crawl(mut self,mesh:&M,trajectory:&Trajectory,lower_bound:Bound<&Time>,upper_bound:Bound<&Time>)->CrawlResult<M>{
let mut lower_bound=lower_bound.map(|&t|into_giga_time(t,trajectory.time));
let upper_bound=upper_bound.map(|&t|into_giga_time(t,trajectory.time));
for _ in 0..20{
match self.next_transition(mesh,relative_body,lower_bound,upper_bound){
match self.next_transition(mesh,trajectory,lower_bound,upper_bound){
Transition::Miss=>return CrawlResult::Miss(self),
Transition::Next(next_fev,next_time)=>(self,lower_bound)=(next_fev,Bound::Included(next_time)),
Transition::Hit(face,time)=>return CrawlResult::Hit(face,time),

View File

@@ -1,7 +1,10 @@
mod body;
mod push_solve;
mod face_crawler;
mod mesh_query;
mod minkowski;
mod model;
mod push_solve;
mod minimum_difference;
pub mod physics;

View File

@@ -0,0 +1,56 @@
pub enum FEV<M:MeshTopology>{
Vert(M::Vert),
Edge(M::Edge),
Face(M::Face),
}
pub trait UndirectedEdge{
type DirectedEdge:DirectedEdge<UndirectedEdge=Self>;
fn as_directed(self,parity:bool)->Self::DirectedEdge;
}
pub trait DirectedEdge{
type UndirectedEdge:UndirectedEdge<DirectedEdge=Self>;
fn as_undirected(self)->Self::UndirectedEdge;
fn parity(&self)->bool;
fn reverse(self)->Self
where
Self:Sized
{
let parity=!self.parity();
self.as_undirected().as_directed(parity)
}
}
pub trait MeshTopology{
type Face;
type Edge:UndirectedEdge<DirectedEdge=Self::DirectedEdge>;
type DirectedEdge:DirectedEdge<UndirectedEdge=Self::Edge>;
type Vert;
fn for_each_vert_edge(&self,vert_id:Self::Vert,f:impl FnMut(Self::DirectedEdge));
fn for_each_vert_face(&self,vert_id:Self::Vert,f:impl FnMut(Self::Face));
fn edge_faces(&self,edge_id:Self::Edge)->impl AsRef<[Self::Face;2]>;
fn edge_verts(&self,edge_id:Self::Edge)->impl AsRef<[Self::Vert;2]>;
#[expect(unused)]
fn for_each_face_vert(&self,face_id:Self::Face,f:impl FnMut(Self::Vert));
fn for_each_face_edge(&self,face_id:Self::Face,f:impl FnMut(Self::DirectedEdge));
}
// Make face_nd d value relative
// euclidean point?
// Simplex physics
// Directed edge necessary?
// recursive for_each function calls
// define faces from vertices (Fixed<2> vs Fixed<3>)
pub trait MeshQuery:MeshTopology{
type Position;
type Direction;
type Normal;
type Offset;
fn vert(&self,vert_id:Self::Vert)->Self::Position;
fn farthest_vert(&self,dir:Self::Direction)->Self::Vert;
/// This must return a point inside the mesh.
fn hint_point(&self)->Self::Position;
fn face_nd(&self,face_id:Self::Face)->(Self::Normal,Self::Offset);
fn edge_n(&self,edge_id:Self::Edge)->Self::Direction;
fn directed_edge_n(&self,directed_edge_id:Self::DirectedEdge)->Self::Direction;
}

View File

@@ -0,0 +1,923 @@
use strafesnet_common::integer::vec3;
use strafesnet_common::integer::vec3::Vector3;
use strafesnet_common::integer::{Fixed,Planar64,Planar64Vec3};
use crate::mesh_query::{FEV,DirectedEdge,MeshQuery,MeshTopology};
// TODO: remove mesh invert
use crate::minkowski::{MinkowskiMesh,MinkowskiVert};
// This algorithm is based on Lua code
// written by Trey Reynolds in 2021
type Simplex<const N:usize,Vert>=[Vert;N];
#[derive(Clone,Copy)]
enum Simplex1_3<Vert>{
Simplex1(Simplex<1,Vert>),
Simplex2(Simplex<2,Vert>),
Simplex3(Simplex<3,Vert>),
}
impl<Vert> Simplex1_3<Vert>{
fn push_front(self,v:Vert)->Simplex2_4<Vert>{
match self{
Simplex1_3::Simplex1([v0])=>Simplex2_4::Simplex2([v,v0]),
Simplex1_3::Simplex2([v0,v1])=>Simplex2_4::Simplex3([v,v0,v1]),
Simplex1_3::Simplex3([v0,v1,v2])=>Simplex2_4::Simplex4([v,v0,v1,v2]),
}
}
}
#[derive(Clone,Copy)]
enum Simplex2_4<Vert>{
Simplex2(Simplex<2,Vert>),
Simplex3(Simplex<3,Vert>),
Simplex4(Simplex<4,Vert>),
}
/*
local function absDet(r, u, v, w)
if w then
return math.abs((u - r):Cross(v - r):Dot(w - r))
elseif v then
return (u - r):Cross(v - r).magnitude
elseif u then
return (u - r).magnitude
else
return 1
end
end
*/
impl<Vert> Simplex2_4<Vert>{
fn det_is_zero<M:MeshQuery<Vert=Vert,Position=Planar64Vec3>>(self,mesh:&M)->bool{
match self{
Self::Simplex4([p0,p1,p2,p3])=>{
let p0=mesh.vert(p0);
let p1=mesh.vert(p1);
let p2=mesh.vert(p2);
let p3=mesh.vert(p3);
(p1-p0).cross(p2-p0).dot(p3-p0)==Fixed::ZERO
},
Self::Simplex3([p0,p1,p2])=>{
let p0=mesh.vert(p0);
let p1=mesh.vert(p1);
let p2=mesh.vert(p2);
(p1-p0).cross(p2-p0)==vec3::zero()
},
Self::Simplex2([p0,p1])=>{
let p0=mesh.vert(p0);
let p1=mesh.vert(p1);
p1-p0==vec3::zero()
}
}
}
}
/*
local function choosePerpendicularDirection(d)
local x, y, z = d.x, d.y, d.z
local best = math.min(x*x, y*y, z*z)
if x*x == best then
return Vector3.new(y*y + z*z, -x*y, -x*z)
elseif y*y == best then
return Vector3.new(-x*y, x*x + z*z, -y*z)
else
return Vector3.new(-x*z, -y*z, x*x + y*y)
end
end
*/
fn choose_perpendicular_direction(d:Planar64Vec3)->Planar64Vec3{
let x=d.x.abs();
let y=d.y.abs();
let z=d.z.abs();
if x<y&&x<z{
Vector3::new([Fixed::ZERO,-d.z,d.y])
}else if y<z{
Vector3::new([d.z,Fixed::ZERO,-d.x])
}else{
Vector3::new([-d.y,d.x,Fixed::ZERO])
}
}
const fn choose_any_direction()->Planar64Vec3{
vec3::X
}
fn narrow_dir2(dir:Vector3<Fixed<2,64>>)->Planar64Vec3{
if dir==vec3::zero(){
return dir.narrow_1().unwrap();
}
let x=dir.x.as_bits().unsigned_abs().bits();
let y=dir.y.as_bits().unsigned_abs().bits();
let z=dir.z.as_bits().unsigned_abs().bits();
let big=x.max(y).max(z);
const MAX_BITS:u32=64+31;
if MAX_BITS<big{
dir>>(big-MAX_BITS)
}else{
dir
}.narrow_1().unwrap()
}
fn narrow_dir3(dir:Vector3<Fixed<3,96>>)->Planar64Vec3{
if dir==vec3::zero(){
return dir.narrow_1().unwrap();
}
let x=dir.x.as_bits().unsigned_abs().bits();
let y=dir.y.as_bits().unsigned_abs().bits();
let z=dir.z.as_bits().unsigned_abs().bits();
let big=x.max(y).max(z);
const MAX_BITS:u32=96+31;
if MAX_BITS<big{
dir>>(big-MAX_BITS)
}else{
dir
}.narrow_1().unwrap()
}
fn reduce1<M:MeshQuery<Position=Planar64Vec3>>(
[v0]:Simplex<1,M::Vert>,
mesh:&M,
point:Planar64Vec3,
)->Reduced<M::Vert>
where M::Vert:Copy,
{
// --debug.profilebegin("reduceSimplex0")
// local a = a1 - a0
let p0=mesh.vert(v0);
// local p = -a
let p=-(p0+point);
// local direction = p
let mut dir=p;
// if direction.magnitude == 0 then
// direction = chooseAnyDirection()
if dir==vec3::zero(){
dir=choose_any_direction();
}
// return direction, a0, a1
Reduced{
dir,
simplex:Simplex1_3::Simplex1([v0]),
}
}
// local function reduceSimplex1(a0, a1, b0, b1)
fn reduce2<M:MeshQuery<Position=Planar64Vec3>>(
[v0,v1]:Simplex<2,M::Vert>,
mesh:&M,
point:Planar64Vec3,
)->Reduced<M::Vert>
where
M::Vert:Copy
{
// --debug.profilebegin("reduceSimplex1")
// local a = a1 - a0
// local b = b1 - b0
let p0=mesh.vert(v0);
let p1=mesh.vert(v1);
// local p = -a
// local u = b - a
let p=-(p0+point);
let u=p1-p0;
// -- modify to take into account the radiuses
// local p_u = p:Dot(u)
let p_u=p.dot(u);
// if p_u >= 0 then
if !p_u.is_negative(){
// local direction = u:Cross(p):Cross(u)
let direction=u.cross(p).cross(u);
// if direction.magnitude == 0 then
if direction==vec3::zero(){
return Reduced{
dir:choose_perpendicular_direction(u),
simplex:Simplex1_3::Simplex2([v0,v1]),
};
}
// -- modify the direction to take into account a0R and b0R
// return direction, a0, a1, b0, b1
return Reduced{
dir:narrow_dir3(direction),
simplex:Simplex1_3::Simplex2([v0,v1]),
};
}
// local direction = p
let mut dir=p;
// if direction.magnitude == 0 then
if dir==vec3::zero(){
dir=choose_perpendicular_direction(u);
}
// return direction, a0, a1
Reduced{
dir,
simplex:Simplex1_3::Simplex1([v0]),
}
}
// local function reduceSimplex2(a0, a1, b0, b1, c0, c1)
fn reduce3<M:MeshQuery<Position=Planar64Vec3>>(
[v0,mut v1,v2]:Simplex<3,M::Vert>,
mesh:&M,
point:Planar64Vec3,
)->Reduced<M::Vert>
where
M::Vert:Copy
{
// --debug.profilebegin("reduceSimplex2")
// local a = a1 - a0
// local b = b1 - b0
// local c = c1 - c0
let p0=mesh.vert(v0);
let p1=mesh.vert(v1);
let p2=mesh.vert(v2);
// local p = -a
// local u = b - a
// local v = c - a
let p=-(p0+point);
let mut u=p1-p0;
let v=p2-p0;
// local uv = u:Cross(v)
// local up = u:Cross(p)
// local pv = p:Cross(v)
// local uv_up = uv:Dot(up)
// local uv_pv = uv:Dot(pv)
let mut uv=u.cross(v);
let mut up=u.cross(p);
let pv=p.cross(v);
let uv_up=uv.dot(up);
let uv_pv=uv.dot(pv);
// if uv_up >= 0 and uv_pv >= 0 then
if !uv_up.is_negative()&&!uv_pv.is_negative(){
// local uvp = uv:Dot(p)
let uvp=uv.dot(p);
// local direction = uvp < 0 and -uv or uv
let direction=if uvp.is_negative(){
-uv
}else{
uv
};
// return direction, a0, a1, b0, b1, c0, c1
return Reduced{
dir:narrow_dir2(direction),
simplex:Simplex1_3::Simplex3([v0,v1,v2]),
};
}
// local u_u = u:Dot(u)
// local v_v = v:Dot(v)
// local uDist = uv_up/(u_u*v.magnitude)
// local vDist = uv_pv/(v_v*u.magnitude)
// local minDist2 = math.min(uDist, vDist)
let u_dist=uv_up*v.length();
let v_dist=uv_pv*u.length();
// if vDist == minDist2 then
if v_dist<u_dist{
u=v;
up=-pv;
uv=-uv;
// b0 = c0
// b1 = c1
v1=v2;
}
// local p_u = p:Dot(u)
let p_u=p.dot(u);
// if p_u >= 0 then
if !p_u.is_negative(){
// local direction = up:Cross(u)
let direction=up.cross(u);
// if direction.magnitude == 0 then
if direction==vec3::zero(){
// direction = uv
return Reduced{
dir:narrow_dir2(uv),
simplex:Simplex1_3::Simplex2([v0,v1]),
};
}
// return direction, a0, a1, b0, b1
return Reduced{
dir:narrow_dir3(direction),
simplex:Simplex1_3::Simplex2([v0,v1]),
};
}
// local direction = p
let dir=p;
// if direction.magnitude == 0 then
if dir==vec3::zero(){
// direction = uv
return Reduced{
dir:narrow_dir2(uv),
simplex:Simplex1_3::Simplex1([v0]),
};
}
// return direction, a0, a0
Reduced{
dir,
simplex:Simplex1_3::Simplex1([v0]),
}
}
// local function reduceSimplex3(a0, a1, b0, b1, c0, c1, d0, d1)
fn reduce4<M:MeshQuery<Position=Planar64Vec3>>(
[v0,mut v1,mut v2,v3]:Simplex<4,M::Vert>,
mesh:&M,
point:Planar64Vec3,
)->Reduce<M::Vert>
where
M::Vert:Copy
{
// --debug.profilebegin("reduceSimplex3")
// local a = a1 - a0
// local b = b1 - b0
// local c = c1 - c0
// local d = d1 - d0
let p0=mesh.vert(v0);
let p1=mesh.vert(v1);
let p2=mesh.vert(v2);
let p3=mesh.vert(v3);
// local p = -a
// local u = b - a
// local v = c - a
// local w = d - a
let p=-(p0+point);
let mut u=p1-p0;
let mut v=p2-p0;
let w=p3-p0;
// local uv = u:Cross(v)
// local vw = v:Cross(w)
// local wu = w:Cross(u)
// local uvw = uv:Dot(w)
// local pvw = vw:Dot(p)
// local upw = wu:Dot(p)
// local uvp = uv:Dot(p)
let mut uv=u.cross(v);
let vw=v.cross(w);
let wu=w.cross(u);
let uv_w=uv.dot(w);
let pv_w=vw.dot(p);
let up_w=wu.dot(p);
let uv_p=uv.dot(p);
// if pvw/uvw >= 0 and upw/uvw >= 0 and uvp/uvw >= 0 then
if !pv_w.div_sign(uv_w).is_negative()
&&!up_w.div_sign(uv_w).is_negative()
&&!uv_p.div_sign(uv_w).is_negative(){
// origin is contained, this is a positive detection
// local direction = Vector3.new(0, 0, 0)
// return direction, a0, a1, b0, b1, c0, c1, d0, d1
return Reduce::Escape([v0,v1,v2,v3]);
}
// local uvwSign = uvw < 0 and -1 or uvw > 0 and 1 or 0
// local uvDist = uvp*uvwSign/uv.magnitude
// local vwDist = pvw*uvwSign/vw.magnitude
// local wuDist = upw*uvwSign/wu.magnitude
// local minDist3 = math.min(uvDist, vwDist, wuDist)
let uv_dist=uv_p.mul_sign(uv_w);
let vw_dist=pv_w.mul_sign(uv_w);
let wu_dist=up_w.mul_sign(uv_w);
let wu_len=wu.length();
let uv_len=uv.length();
let vw_len=vw.length();
if vw_dist*wu_len<wu_dist*vw_len{
// if vwDist == minDist3 then
if vw_dist*uv_len<uv_dist*vw_len{
(u,v)=(v,w);
uv=vw;
// uv_p=pv_w; // unused
// b0, c0 = c0, d0
// b1, c1 = c1, d1
(v1,v2)=(v2,v3);
}
}else{
// elseif wuDist == minDist3 then
if wu_dist*uv_len<uv_dist*wu_len{
(u,v)=(w,u);
uv=wu;
// uv_p=up_w; // unused
// b0, c0 = d0, b0
// b1, c1 = d1, b1
// before [a,b,c,d]
(v1,v2)=(v3,v1);
// after [a,d,b]
}
}
// local up = u:Cross(p)
// local pv = p:Cross(v)
// local uv_up = uv:Dot(up)
// local uv_pv = uv:Dot(pv)
let mut up=u.cross(p);
let pv=p.cross(v);
let uv_up=uv.dot(up);
let uv_pv=uv.dot(pv);
// if uv_up >= 0 and uv_pv >= 0 then
if !uv_up.is_negative()&&!uv_pv.is_negative(){
// local direction = uvw < 0 and uv or -uv
// return direction, a0, a1, b0, b1, c0, c1
let dir=if uv_w.is_negative(){
narrow_dir2(uv)
}else{
narrow_dir2(-uv)
};
return Reduce::Reduced(Reduced{
dir,
simplex:Simplex1_3::Simplex3([v0,v1,v2]),
});
}
// local u_u = u:Dot(u)
// local v_v = v:Dot(v)
// local uDist = uv_up/(u_u*v.magnitude)
// local vDist = uv_pv/(v_v*u.magnitude)
// local minDist2 = math.min(uDist, vDist)
let u_dist=uv_up*v.length();
let v_dist=uv_pv*u.length();
// if vDist == minDist2 then
if v_dist<u_dist{
u=v;
up=-pv;
uv=-uv;
// b0 = c0
// b1 = c1
v1=v2;
}
// local p_u = p:Dot(u)
let p_u=p.dot(u);
// if p_u >= 0 then
if !p_u.is_negative(){
// local direction = up:Cross(u)
let direction=up.cross(u);
// if direction.magnitude == 0 then
if direction==vec3::zero(){
// direction = uvw < 0 and uv or -uv
// return direction, a0, a1, b0, b1
let dir=if uv_w.is_negative(){
narrow_dir2(uv)
}else{
narrow_dir2(-uv)
};
return Reduce::Reduced(Reduced{
dir,
simplex:Simplex1_3::Simplex2([v0,v1]),
});
}
// return direction, a0, a1, b0, b1
return Reduce::Reduced(Reduced{
dir:narrow_dir3(direction),
simplex:Simplex1_3::Simplex2([v0,v1]),
});
}
// local direction = p
let dir=p;
// if direction.magnitude == 0 then
if dir==vec3::zero(){
// direction = uvw < 0 and uv or -uv
let dir=if uv_w.is_negative(){
narrow_dir2(uv)
}else{
narrow_dir2(-uv)
};
return Reduce::Reduced(Reduced{
dir,
simplex:Simplex1_3::Simplex1([v0]),
});
}
// return direction, a0, a1
Reduce::Reduced(Reduced{
dir,
simplex:Simplex1_3::Simplex1([v0]),
})
}
struct Reduced<Vert>{
dir:Planar64Vec3,
simplex:Simplex1_3<Vert>,
}
enum Reduce<Vert>{
Escape(Simplex<4,Vert>),
Reduced(Reduced<Vert>),
}
impl<Vert> Simplex2_4<Vert>{
fn reduce<M:MeshQuery<Vert=Vert,Position=Planar64Vec3>>(self,mesh:&M,point:Planar64Vec3)->Reduce<Vert>
where
M::Vert:Copy
{
match self{
Self::Simplex2(simplex)=>Reduce::Reduced(reduce2(simplex,mesh,point)),
Self::Simplex3(simplex)=>Reduce::Reduced(reduce3(simplex,mesh,point)),
Self::Simplex4(simplex)=>reduce4(simplex,mesh,point),
}
}
}
//infinity fev algorithm state transition
#[derive(Debug)]
enum Transition<Vert>{
Done,//found closest vert, no edges are better
Vert(Vert),//transition to vert
}
enum EV<M:MeshTopology>{
Vert(M::Vert),
Edge(M::Edge),
}
impl<M:MeshTopology> From<EV<M>> for FEV<M>{
fn from(value:EV<M>)->Self{
match value{
EV::Vert(minkowski_vert)=>FEV::Vert(minkowski_vert),
EV::Edge(minkowski_edge)=>FEV::Edge(minkowski_edge),
}
}
}
trait Contains{
fn contains(&self,point:Planar64Vec3)->bool;
}
// convenience type to check if a point is within some threshold of a plane.
struct ThickPlane{
point:Planar64Vec3,
normal:Vector3<Fixed<2,64>>,
epsilon:Fixed<3,96>,
}
impl ThickPlane{
fn new<M:MeshQuery<Position=Planar64Vec3>>(mesh:&M,[v0,v1,v2]:Simplex<3,M::Vert>)->Self{
let p0=mesh.vert(v0);
let p1=mesh.vert(v1);
let p2=mesh.vert(v2);
let point=p0;
let normal=(p1-p0).cross(p2-p0);
// Allow ~ 2*sqrt(3) units of thickness on the plane
// This is to account for the variance of two voxels across the longest diagonal
let epsilon=(normal.length()*(Planar64::EPSILON*3)).wrap_3();
Self{point,normal,epsilon}
}
}
impl Contains for ThickPlane{
fn contains(&self,point:Planar64Vec3)->bool{
(point-self.point).dot(self.normal).abs()<=self.epsilon
}
}
struct ThickLine{
point:Planar64Vec3,
dir:Planar64Vec3,
epsilon:Fixed<4,128>,
}
impl ThickLine{
fn new<M:MeshQuery<Position=Planar64Vec3>>(mesh:&M,[v0,v1]:Simplex<2,M::Vert>)->Self{
let p0=mesh.vert(v0);
let p1=mesh.vert(v1);
let point=p0;
let dir=p1-p0;
// Allow ~ 2*sqrt(3) units of thickness on the plane
// This is to account for the variance of two voxels across the longest diagonal
let epsilon=(dir.length_squared()*(Planar64::EPSILON*3)).widen_4();
Self{point,dir,epsilon}
}
}
impl Contains for ThickLine{
fn contains(&self,point:Planar64Vec3)->bool{
(point-self.point).cross(self.dir).length_squared()<=self.epsilon
}
}
struct EVFinder<'a,M,C>{
mesh:&'a M,
constraint:C,
best_distance_squared:Fixed<2,64>,
}
impl<M:MeshQuery<Position=Planar64Vec3>,C:Contains> EVFinder<'_,M,C>
where
M::Vert:Copy,
M::DirectedEdge:Copy,
{
fn next_transition_vert(&mut self,vert_id:M::Vert,point:Planar64Vec3)->Transition<M::Vert>{
let mut best_transition=Transition::Done;
self.mesh.for_each_vert_edge(vert_id,|directed_edge_id|{
//test if this edge's opposite vertex closer
let edge_verts=self.mesh.edge_verts(directed_edge_id.as_undirected());
//select opposite vertex
let test_vert_id=edge_verts.as_ref()[directed_edge_id.parity() as usize];
let test_pos=self.mesh.vert(test_vert_id);
let diff=point-test_pos;
let distance_squared=diff.dot(diff);
// ensure test_vert_id is coplanar to simplex
if distance_squared<self.best_distance_squared&&self.constraint.contains(test_pos){
best_transition=Transition::Vert(test_vert_id);
self.best_distance_squared=distance_squared;
}
});
best_transition
}
fn final_ev(&mut self,vert_id:M::Vert,point:Planar64Vec3)->EV<M>{
let mut best_transition=EV::Vert(vert_id);
let vert_pos=self.mesh.vert(vert_id);
let diff=point-vert_pos;
self.mesh.for_each_vert_edge(vert_id,|directed_edge_id|{
//test if this edge is closer
let edge_verts=self.mesh.edge_verts(directed_edge_id.as_undirected());
let test_vert_id=edge_verts.as_ref()[directed_edge_id.parity() as usize];
let test_pos=self.mesh.vert(test_vert_id);
let edge_n=test_pos-vert_pos;
let d=edge_n.dot(diff);
//test the edge
let edge_nn=edge_n.dot(edge_n);
// ensure edge contains closest point and directed_edge_id is coplanar to simplex
if !d.is_negative()&&d<=edge_nn&&self.constraint.contains(test_pos){
let distance_squared={
let c=diff.cross(edge_n);
//wrap for speed
(c.dot(c)/edge_nn).divide().wrap_2()
};
if distance_squared<=self.best_distance_squared{
best_transition=EV::Edge(directed_edge_id.as_undirected());
self.best_distance_squared=distance_squared;
}
}
});
best_transition
}
fn crawl_boundaries(&mut self,mut vert_id:M::Vert,point:Planar64Vec3)->EV<M>
where
M::Vert:Copy
{
loop{
match self.next_transition_vert(vert_id,point){
Transition::Done=>return self.final_ev(vert_id,point),
Transition::Vert(new_vert_id)=>vert_id=new_vert_id,
}
}
}
}
/// This function hops along parallel vertices until it finds the EV which contains the closest point to `point`.
fn crawl_to_closest_ev<M:MeshQuery<Position=Planar64Vec3>>(mesh:&M,simplex:Simplex<2,M::Vert>,point:Planar64Vec3)->EV<M>
where
M::Vert:Copy,
M::DirectedEdge:Copy,
{
// naively start at the closest vertex
// the closest vertex is not necessarily the one with the fewest boundary hops
// but it doesn't matter, we will get there regardless.
let (vert_id,best_distance_squared)=simplex.into_iter().map(|vert_id|{
let diff=point-mesh.vert(vert_id);
(vert_id,diff.dot(diff))
}).min_by_key(|&(_,d)|d).unwrap();
let constraint=ThickLine::new(mesh,simplex);
let mut finder=EVFinder{constraint,mesh,best_distance_squared};
//start on any vertex
//cross uncrossable vertex-edge boundaries until you find the closest vertex or edge
//cross edge-face boundary if it's uncrossable
finder.crawl_boundaries(vert_id,point)
}
/// This function hops along connected vertices until it finds the FEV which contains the closest point to `point`.
fn crawl_to_closest_fev<'a>(mesh:&MinkowskiMesh<'a>,simplex:Simplex<3,MinkowskiVert>,point:Planar64Vec3)->FEV::<MinkowskiMesh<'a>>{
// naively start at the closest vertex
// the closest vertex is not necessarily the one with the fewest boundary hops
// but it doesn't matter, we will get there regardless.
let (vert_id,best_distance_squared)=simplex.into_iter().map(|vert_id|{
let diff=point-mesh.vert(vert_id);
(vert_id,diff.dot(diff))
}).min_by_key(|&(_,d)|d).unwrap();
let constraint=ThickPlane::new(mesh,simplex);
let mut finder=EVFinder{constraint,mesh,best_distance_squared};
//start on any vertex
//cross uncrossable vertex-edge boundaries until you find the closest vertex or edge
//cross edge-face boundary if it's uncrossable
match finder.crawl_boundaries(vert_id,point){
//if a vert is returned, it is the closest point to the infinity point
EV::Vert(vert_id)=>FEV::Vert(vert_id),
EV::Edge(edge_id)=>{
//cross to face if we are on the wrong side
let edge_n=mesh.edge_n(edge_id);
// point is multiplied by two because vert_sum sums two vertices.
let delta_pos=point*2-{
let &[v0,v1]=mesh.edge_verts(edge_id).as_ref();
mesh.vert(v0)+mesh.vert(v1)
};
for (i,&face_id) in mesh.edge_faces(edge_id).as_ref().iter().enumerate(){
//test if this face is closer
let (face_n,d)=mesh.face_nd(face_id);
//if test point is behind face, the face is invalid
// TODO: find out why I thought of this backwards
if !(face_n.dot(point)-d).is_positive(){
continue;
}
//edge-face boundary nd, n facing out of the face towards the edge
let boundary_n=face_n.cross(edge_n)*(i as i64*2-1);
let boundary_d=boundary_n.dot(delta_pos);
//is test point behind edge, i.e. contained in the face
if !boundary_d.is_positive(){
//both faces cannot pass this condition, return early if one does.
return FEV::Face(face_id);
}
}
FEV::Edge(edge_id)
},
}
}
pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option<FEV<MinkowskiMesh<'a>>>{
const ENABLE_FAST_FAIL:bool=false;
// TODO: remove mesh negation
minimum_difference::<ENABLE_FAST_FAIL,_,_>(&-mesh,point,
// on_exact
|is_intersecting,simplex|{
if is_intersecting{
return None;
}
// Convert simplex to FEV
// Vertices must be inverted since the mesh is inverted
Some(match simplex{
Simplex1_3::Simplex1([v0])=>FEV::Vert(-v0),
Simplex1_3::Simplex2([v0,v1])=>{
// invert
let (v0,v1)=(-v0,-v1);
crawl_to_closest_ev(mesh,[v0,v1],point).into()
},
Simplex1_3::Simplex3([v0,v1,v2])=>{
// invert
let (v0,v1,v2)=(-v0,-v1,-v2);
// Shimmy to the side until you find a face that contains the closest point
// it's ALWAYS representable as a face, but this algorithm may
// return E or V in edge cases but I don't think that will break the face crawler
crawl_to_closest_fev(mesh,[v0,v1,v2],point)
},
})
},
// on_escape
|_simplex|{
// intersection is guaranteed at this point
// local norm, dist, u0, u1, v0, v1, w0, w1 = expand(queryP, queryQ, a0, a1, b0, b1, c0, c1, d0, d1, 1e-5)
// let simplex=refine_to_exact(mesh,simplex);
None
},
// fast_fail value is irrelevant and will never be returned!
||unreachable!()
)
}
pub fn contains_point(mesh:&MinkowskiMesh<'_>,point:Planar64Vec3)->bool{
const ENABLE_FAST_FAIL:bool=true;
// TODO: remove mesh negation
minimum_difference::<ENABLE_FAST_FAIL,_,_>(&-mesh,point,
// on_exact
|is_intersecting,_simplex|{
is_intersecting
},
// on_escape
|_simplex|{
// intersection is guaranteed at this point
true
},
// fast_fail value
||false
)
}
// local function minimumDifference(
// queryP, radiusP,
// queryQ, radiusQ,
// exitRadius, testIntersection
// )
fn minimum_difference<const ENABLE_FAST_FAIL:bool,T,M:MeshQuery<Position=Planar64Vec3,Direction=Planar64Vec3>>(
mesh:&M,
point:Planar64Vec3,
on_exact:impl FnOnce(bool,Simplex1_3<M::Vert>)->T,
on_escape:impl FnOnce(Simplex<4,M::Vert>)->T,
on_fast_fail:impl FnOnce()->T,
)->T
where
M::Vert:Copy
{
// local initialAxis = queryQ() - queryP()
// local new_point_p = queryP(initialAxis)
// local new_point_q = queryQ(-initialAxis)
// local direction, a0, a1, b0, b1, c0, c1, d0, d1
let mut initial_axis=mesh.hint_point()+point;
// degenerate case
if initial_axis==vec3::zero(){
initial_axis=choose_any_direction();
}
let last_point=mesh.farthest_vert(-initial_axis);
// this represents the 'a' value in the commented code
let mut last_pos=mesh.vert(last_point);
let Reduced{dir:mut direction,simplex:mut simplex_small}=reduce1([last_point],mesh,point);
// exitRadius = testIntersection and 0 or exitRadius or 1/0
// for _ = 1, 100 do
loop{
// new_point_p = queryP(-direction)
// new_point_q = queryQ(direction)
// local next_point = new_point_q - new_point_p
let next_point=mesh.farthest_vert(direction);
let next_pos=mesh.vert(next_point);
// if -direction:Dot(next_point) > (exitRadius + radiusP + radiusQ)*direction.magnitude then
if ENABLE_FAST_FAIL&&direction.dot(next_pos+point).is_negative(){
return on_fast_fail();
}
let simplex_big=simplex_small.push_front(next_point);
// if
// direction:Dot(next_point - a) <= 0 or
// absDet(next_point, a, b, c) < 1e-6
if !direction.dot(next_pos-last_pos).is_positive()
||simplex_big.det_is_zero(mesh){
// Found enough information to compute the exact closest point.
// local norm = direction.unit
// local dist = a:Dot(norm)
// local hits = -dist < radiusP + radiusQ
let is_intersecting=(last_pos+point).dot(direction).is_positive();
return on_exact(is_intersecting,simplex_small);
}
// direction, a0, a1, b0, b1, c0, c1, d0, d1 = reduceSimplex(new_point_p, new_point_q, a0, a1, b0, b1, c0, c1)
match simplex_big.reduce(mesh,point){
// if a and b and c and d then
Reduce::Escape(simplex)=>{
// Enough information to conclude that the meshes are intersecting.
// Topology information is computed if needed.
return on_escape(simplex);
},
Reduce::Reduced(reduced)=>{
direction=reduced.dir;
simplex_small=reduced.simplex;
},
}
// next loop this will be a
last_pos=next_pos;
}
}
#[cfg(test)]
mod test{
use super::*;
use crate::model::{PhysicsMesh,PhysicsMeshView};
fn mesh_contains_point(mesh:PhysicsMeshView<'_>,point:Planar64Vec3)->bool{
const ENABLE_FAST_FAIL:bool=true;
// TODO: remove mesh negation
minimum_difference::<ENABLE_FAST_FAIL,_,_>(&mesh,point,
// on_exact
|is_intersecting,_simplex|{
is_intersecting
},
// on_escape
|_simplex|{
// intersection is guaranteed at this point
true
},
// fast_fail value
||false
)
}
#[test]
fn test_cube_points(){
let mesh=PhysicsMesh::unit_cube();
let mesh_view=mesh.complete_mesh_view();
for x in -2..=2{
for y in -2..=2{
for z in -2..=2{
let point=vec3::int(x,y,z)>>1;
assert!(mesh_contains_point(mesh_view,point),"Mesh did not contain point {point}");
}
}
}
}
}

View File

@@ -0,0 +1,407 @@
use core::ops::{Bound,RangeBounds};
use strafesnet_common::integer::{Planar64Vec3,Ratio,Fixed,vec3::Vector3};
use crate::model::into_giga_time;
use crate::model::{SubmeshVertId,SubmeshEdgeId,SubmeshDirectedEdgeId,SubmeshFaceId,TransformedMesh,GigaTime};
use crate::mesh_query::{MeshQuery,MeshTopology,DirectedEdge,UndirectedEdge};
use crate::physics::{Time,Trajectory};
struct AsRefHelper<T>(T);
impl<T> AsRef<T> for AsRefHelper<T>{
fn as_ref(&self)->&T{
&self.0
}
}
//Note that a face on a minkowski mesh refers to a pair of fevs on the meshes it's summed from
//(face,vertex)
//(edge,edge)
//(vertex,face)
#[derive(Clone,Copy,Debug,Eq,PartialEq)]
pub enum MinkowskiVert{
VertVert(SubmeshVertId,SubmeshVertId),
}
// TODO: remove this
impl core::ops::Neg for MinkowskiVert{
type Output=Self;
fn neg(self)->Self::Output{
match self{
MinkowskiVert::VertVert(v0,v1)=>MinkowskiVert::VertVert(v1,v0),
}
}
}
#[derive(Clone,Copy,Debug)]
pub enum MinkowskiEdge{
VertEdge(SubmeshVertId,SubmeshEdgeId),
EdgeVert(SubmeshEdgeId,SubmeshVertId),
//EdgeEdge when edges are parallel
}
impl UndirectedEdge for MinkowskiEdge{
type DirectedEdge=MinkowskiDirectedEdge;
fn as_directed(self,parity:bool)->Self::DirectedEdge{
match self{
MinkowskiEdge::VertEdge(v0,e1)=>MinkowskiDirectedEdge::VertEdge(v0,e1.as_directed(parity)),
MinkowskiEdge::EdgeVert(e0,v1)=>MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),v1),
}
}
}
#[derive(Clone,Copy,Debug)]
pub enum MinkowskiDirectedEdge{
VertEdge(SubmeshVertId,SubmeshDirectedEdgeId),
EdgeVert(SubmeshDirectedEdgeId,SubmeshVertId),
//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(Clone,Copy,Debug,Hash)]
pub enum MinkowskiFace{
VertFace(SubmeshVertId,SubmeshFaceId),
EdgeEdge(SubmeshEdgeId,SubmeshEdgeId,bool),
FaceVert(SubmeshFaceId,SubmeshVertId),
//EdgeFace
//FaceEdge
//FaceFace
}
#[derive(Debug)]
pub struct MinkowskiMesh<'a>{
mesh0:TransformedMesh<'a>,
mesh1:TransformedMesh<'a>,
}
// TODO: remove this
impl<'a> core::ops::Neg for &MinkowskiMesh<'a>{
type Output=MinkowskiMesh<'a>;
fn neg(self)->Self::Output{
MinkowskiMesh::minkowski_sum(self.mesh1,self.mesh0)
}
}
impl MinkowskiMesh<'_>{
pub fn minkowski_sum<'a>(mesh0:TransformedMesh<'a>,mesh1:TransformedMesh<'a>)->MinkowskiMesh<'a>{
MinkowskiMesh{
mesh0,
mesh1,
}
}
pub fn predict_collision_in(&self,trajectory:&Trajectory,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
let start_position=match range.start_bound(){
Bound::Included(time)=>trajectory.extrapolated_position(*time),
Bound::Excluded(time)=>trajectory.extrapolated_position(*time),
Bound::Unbounded=>trajectory.position,
};
let fev=crate::minimum_difference::closest_fev_not_inside(self,start_position)?;
//continue forwards along the body parabola
fev.crawl(self,trajectory,range.start_bound(),range.end_bound()).hit()
}
pub fn predict_collision_out(&self,trajectory:&Trajectory,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
let (lower_bound,upper_bound)=(range.start_bound(),range.end_bound());
// TODO: handle unbounded collision using infinity fev
let start_position=match upper_bound{
Bound::Included(time)=>trajectory.extrapolated_position(*time),
Bound::Excluded(time)=>trajectory.extrapolated_position(*time),
Bound::Unbounded=>trajectory.position,
};
let fev=crate::minimum_difference::closest_fev_not_inside(self,start_position)?;
// swap and negate bounds to do a time inversion
let (lower_bound,upper_bound)=(upper_bound.map(|&t|-t),lower_bound.map(|&t|-t));
let time_reversed_trajectory=-trajectory;
//continue backwards along the body parabola
fev.crawl(self,&time_reversed_trajectory,lower_bound.as_ref(),upper_bound.as_ref()).hit()
//no need to test -time<time_limit because of the first step
.map(|(face,time)|(face,-time))
}
pub fn predict_collision_face_out(&self,trajectory:&Trajectory,range:impl RangeBounds<Time>,contact_face_id:MinkowskiFace)->Option<(MinkowskiDirectedEdge,GigaTime)>{
// TODO: make better
use crate::face_crawler::{low,upp};
//no algorithm needed, there is only one state and two cases (Edge,None)
//determine when it passes an edge ("sliding off" case)
let start_time=range.start_bound().map(|&t|(t-trajectory.time).to_ratio());
let mut best_time=range.end_bound().map(|&t|into_giga_time(t,trajectory.time));
let mut best_edge=None;
let face_n=self.face_nd(contact_face_id).0;
self.for_each_face_edge(contact_face_id,|directed_edge_id|{
let edge_n=self.directed_edge_n(directed_edge_id);
//f x e points in
let n=face_n.cross(edge_n);
let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
let d=n.dot(self.vert(v0)+self.vert(v1));
//WARNING! d outside of *2
//WARNING: truncated precision
//wrap for speed
for dt in Fixed::<4,128>::zeroes2(((n.dot(trajectory.position))*2-d).wrap_4(),n.dot(trajectory.velocity).wrap_4()*2,n.dot(trajectory.acceleration).wrap_4()){
if low(&start_time,&dt)&&upp(&dt,&best_time)&&n.dot(trajectory.extrapolated_velocity_ratio_dt(dt)).is_negative(){
best_time=Bound::Included(dt);
best_edge=Some((directed_edge_id,dt));
break;
}
}
});
best_edge
}
pub fn contains_point(&self,point:Planar64Vec3)->bool{
crate::minimum_difference::contains_point(self,point)
}
}
impl MeshQuery for MinkowskiMesh<'_>{
type Direction=Planar64Vec3;
type Position=Planar64Vec3;
type Normal=Vector3<Fixed<3,96>>;
type Offset=Fixed<4,128>;
// TODO: relative d
fn face_nd(&self,face_id:MinkowskiFace)->(Self::Normal,Self::Offset){
match face_id{
MinkowskiFace::VertFace(v0,f1)=>{
let (n,d)=self.mesh1.face_nd(f1);
(-n,d-n.dot(self.mesh0.vert(v0)))
},
MinkowskiFace::EdgeEdge(e0,e1,parity)=>{
let edge0_n=self.mesh0.edge_n(e0);
let edge1_n=self.mesh1.edge_n(e1);
let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).as_ref();
let &[e1v0,e1v1]=self.mesh1.edge_verts(e1).as_ref();
let n=edge0_n.cross(edge1_n);
let e0d=n.dot(self.mesh0.vert(e0v0)+self.mesh0.vert(e0v1));
let e1d=n.dot(self.mesh1.vert(e1v0)+self.mesh1.vert(e1v1));
((n*(parity as i64*4-2)).widen_3(),((e0d-e1d)*(parity as i64*2-1)).widen_4())
},
MinkowskiFace::FaceVert(f0,v1)=>{
let (n,d)=self.mesh0.face_nd(f0);
(n,d-n.dot(self.mesh1.vert(v1)))
},
}
}
fn vert(&self,vert_id:MinkowskiVert)->Planar64Vec3{
match vert_id{
MinkowskiVert::VertVert(v0,v1)=>{
self.mesh0.vert(v0)-self.mesh1.vert(v1)
},
}
}
fn hint_point(&self)->Planar64Vec3{
self.mesh0.hint_point()-self.mesh1.hint_point()
}
fn farthest_vert(&self,dir:Planar64Vec3)->MinkowskiVert{
MinkowskiVert::VertVert(self.mesh0.farthest_vert(dir),self.mesh1.farthest_vert(-dir))
}
fn edge_n(&self,edge_id:Self::Edge)->Self::Direction{
let &[v0,v1]=self.edge_verts(edge_id).as_ref();
self.vert(v1)-self.vert(v0)
}
fn directed_edge_n(&self,directed_edge_id:Self::DirectedEdge)->Self::Direction{
let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
(self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1)
}
}
impl MeshTopology for MinkowskiMesh<'_>{
type Face=MinkowskiFace;
type Edge=MinkowskiEdge;
type DirectedEdge=MinkowskiDirectedEdge;
type Vert=MinkowskiVert;
fn for_each_vert_edge(&self,vert_id:Self::Vert,mut f:impl FnMut(Self::DirectedEdge)){
match vert_id{
MinkowskiVert::VertVert(v0,v1)=>{
//detect shared volume when the other mesh is mirrored along a test edge dir
let v0f={
let mut faces=Vec::new();
self.mesh0.for_each_vert_face(v0,|face|faces.push(face));
faces
};
let v1f={
let mut faces=Vec::new();
self.mesh1.for_each_vert_face(v1,|face|faces.push(face));
faces
};
let v0f_n:Vec<_>=v0f.iter().map(|&face_id|self.mesh0.face_nd(face_id).0).collect();
let v1f_n:Vec<_>=v1f.iter().map(|&face_id|self.mesh1.face_nd(face_id).0).collect();
// scratch vector
let mut face_normals=Vec::with_capacity(v0f.len()+v1f.len());
face_normals.clone_from(&v0f_n);
self.mesh0.for_each_vert_edge(v0,|directed_edge_id|{
let n=self.mesh0.directed_edge_n(directed_edge_id);
let nn=n.dot(n);
// TODO: there's gotta be a better way to do this
// drop faces beyond v0f_n
face_normals.truncate(v0f.len());
// make a set of faces from mesh0's perspective
for face_n in &v1f_n{
//add reflected mesh1 faces
//wrap for speed
face_normals.push(*face_n-(n*face_n.dot(n)*2/nn).divide().wrap_3());
}
if is_empty_volume(&face_normals){
f(MinkowskiDirectedEdge::EdgeVert(directed_edge_id,v1));
}
});
face_normals.clone_from(&v1f_n);
self.mesh1.for_each_vert_edge(v1,|directed_edge_id|{
let n=self.mesh1.directed_edge_n(directed_edge_id);
let nn=n.dot(n);
// drop faces beyond v1f_n
face_normals.truncate(v1f.len());
// make a set of faces from mesh1's perspective
for face_n in &v0f_n{
//wrap for speed
face_normals.push(*face_n-(n*face_n.dot(n)*2/nn).divide().wrap_3());
}
if is_empty_volume(&face_normals){
f(MinkowskiDirectedEdge::VertEdge(v0,directed_edge_id));
}
});
},
}
}
fn for_each_vert_face(&self,_vert_id:Self::Vert,_f:impl FnMut(Self::Face)){
unimplemented!()
}
fn edge_faces(&self,edge_id:Self::Edge)->impl AsRef<[Self::Face;2]>{
match edge_id{
MinkowskiEdge::VertEdge(v0,e1)=>{
//faces are listed backwards from the minkowski mesh
let v0e={
let mut edges=Vec::new();
self.mesh0.for_each_vert_edge(v0,|edge|edges.push(edge));
edges
};
let &[e1f0,e1f1]=self.mesh1.edge_faces(e1).as_ref();
AsRefHelper([(e1f1,false),(e1f0,true)].map(|(edge_face_id1,face_parity)|{
let mut best_edge=None;
let mut best_d:Ratio<Fixed<8,256>,Fixed<8,256>>=Ratio::new(Fixed::ZERO,Fixed::ONE);
let edge_face1_n=self.mesh1.face_nd(edge_face_id1).0;
let edge_face1_nn=edge_face1_n.dot(edge_face1_n);
for &directed_edge_id0 in &v0e{
let edge0_n=self.mesh0.directed_edge_n(directed_edge_id0);
//must be behind other face.
let d=edge_face1_n.dot(edge0_n);
if d.is_negative(){
let edge0_nn=edge0_n.dot(edge0_n);
// Assume not every number is huge
// TODO: revisit this
let dd=(d*d)/(edge_face1_nn*edge0_nn);
if best_d<dd{
best_d=dd;
best_edge=Some(directed_edge_id0);
}
}
}
best_edge.map_or(
MinkowskiFace::VertFace(v0,edge_face_id1),
|directed_edge_id0|MinkowskiFace::EdgeEdge(directed_edge_id0.as_undirected(),e1,directed_edge_id0.parity()^face_parity)
)
}))
},
MinkowskiEdge::EdgeVert(e0,v1)=>{
//tracking index with an external variable because .enumerate() is not available
let v1e={
let mut edges=Vec::new();
self.mesh1.for_each_vert_edge(v1,|edge|edges.push(edge));
edges
};
let &[e0f0,e0f1]=self.mesh0.edge_faces(e0).as_ref();
AsRefHelper([(e0f0,true),(e0f1,false)].map(|(edge_face_id0,face_parity)|{
let mut best_edge=None;
let mut best_d:Ratio<Fixed<8,256>,Fixed<8,256>>=Ratio::new(Fixed::ZERO,Fixed::ONE);
let edge_face0_n=self.mesh0.face_nd(edge_face_id0).0;
let edge_face0_nn=edge_face0_n.dot(edge_face0_n);
for &directed_edge_id1 in &v1e{
let edge1_n=self.mesh1.directed_edge_n(directed_edge_id1);
let d=edge_face0_n.dot(edge1_n);
if d.is_negative(){
let edge1_nn=edge1_n.dot(edge1_n);
let dd=(d*d)/(edge_face0_nn*edge1_nn);
if best_d<dd{
best_d=dd;
best_edge=Some(directed_edge_id1);
}
}
}
best_edge.map_or(
MinkowskiFace::FaceVert(edge_face_id0,v1),
|directed_edge_id1|MinkowskiFace::EdgeEdge(e0,directed_edge_id1.as_undirected(),directed_edge_id1.parity()^face_parity)
)
}))
},
}
}
fn edge_verts(&self,edge_id:Self::Edge)->impl AsRef<[Self::Vert;2]>{
AsRefHelper(match edge_id{
MinkowskiEdge::VertEdge(v0,e1)=>self.mesh1.edge_verts(e1).as_ref().map(|vert_id1|
MinkowskiVert::VertVert(v0,vert_id1)
),
MinkowskiEdge::EdgeVert(e0,v1)=>self.mesh0.edge_verts(e0).as_ref().map(|vert_id0|
MinkowskiVert::VertVert(vert_id0,v1)
),
})
}
fn for_each_face_vert(&self,_face_id:Self::Face,_f:impl FnMut(Self::Vert)){
unimplemented!()
}
fn for_each_face_edge(&self,face_id:Self::Face,mut f:impl FnMut(Self::DirectedEdge)){
match face_id{
MinkowskiFace::VertFace(v0,f1)=>{
self.mesh1.for_each_face_edge(f1,|edge_id1|
f(MinkowskiDirectedEdge::VertEdge(v0,edge_id1.reverse()))
)
},
MinkowskiFace::EdgeEdge(e0,e1,parity)=>{
let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).as_ref();
let &[e1v0,e1v1]=self.mesh1.edge_verts(e1).as_ref();
//could sort this if ordered edges are needed
//probably just need to reverse this list according to parity
f(MinkowskiDirectedEdge::VertEdge(e0v0,e1.as_directed(parity)));
f(MinkowskiDirectedEdge::EdgeVert(e0.as_directed(!parity),e1v0));
f(MinkowskiDirectedEdge::VertEdge(e0v1,e1.as_directed(!parity)));
f(MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),e1v1));
},
MinkowskiFace::FaceVert(f0,v1)=>{
self.mesh0.for_each_face_edge(f0,|edge_id0|
f(MinkowskiDirectedEdge::EdgeVert(edge_id0,v1))
)
},
}
}
}
fn is_empty_volume(normals:&[Vector3<Fixed<3,96>>])->bool{
let len=normals.len();
for i in 0..len-1{
for j in i+1..len{
let n=normals[i].cross(normals[j]);
let mut d_comp=None;
for k in 0..len{
if k!=i&&k!=j{
let d=n.dot(normals[k]).is_negative();
if let &Some(comp)=&d_comp{
// This is testing if d_comp*d < 0
if comp^d{
return true;
}
}else{
d_comp=Some(d);
}
}
}
}
}
return false;
}
#[test]
fn test_is_empty_volume(){
use strafesnet_common::integer::vec3;
assert!(!is_empty_volume(&[vec3::X.widen_3(),vec3::Y.widen_3(),vec3::Z.widen_3()]));
assert!(is_empty_volume(&[vec3::X.widen_3(),vec3::Y.widen_3(),vec3::Z.widen_3(),vec3::NEG_X.widen_3()]));
}

View File

@@ -1,11 +1,9 @@
use std::collections::{HashSet,HashMap};
use core::ops::{Bound,RangeBounds};
use strafesnet_common::integer::vec3::Vector3;
use strafesnet_common::model::{self,MeshId,PolygonIter};
use strafesnet_common::integer::{self,vec3,Fixed,Planar64,Planar64Vec3,Ratio};
use strafesnet_common::physics::Time;
type Body=crate::body::Body<strafesnet_common::physics::TimeInner>;
use crate::mesh_query::{MeshQuery,MeshTopology,DirectedEdge,UndirectedEdge};
struct AsRefHelper<T>(T);
impl<T> AsRef<T> for AsRefHelper<T>{
@@ -14,20 +12,6 @@ impl<T> AsRef<T> for AsRefHelper<T>{
}
}
pub trait UndirectedEdge{
type DirectedEdge:Copy+DirectedEdge;
fn as_directed(&self,parity:bool)->Self::DirectedEdge;
}
pub trait DirectedEdge{
type UndirectedEdge:Copy+std::fmt::Debug+UndirectedEdge;
fn as_undirected(&self)->Self::UndirectedEdge;
fn parity(&self)->bool;
//this is stupid but may work fine
fn reverse(&self)-><<Self as DirectedEdge>::UndirectedEdge as UndirectedEdge>::DirectedEdge{
self.as_undirected().as_directed(!self.parity())
}
}
#[derive(Debug,Clone,Copy,Hash,id::Id,Eq,PartialEq)]
pub struct MeshVertId(u32);
#[derive(Debug,Clone,Copy,Hash,id::Id,Eq,PartialEq)]
@@ -45,13 +29,13 @@ pub struct SubmeshFaceId(u32);
impl UndirectedEdge for SubmeshEdgeId{
type DirectedEdge=SubmeshDirectedEdgeId;
fn as_directed(&self,parity:bool)->SubmeshDirectedEdgeId{
fn as_directed(self,parity:bool)->SubmeshDirectedEdgeId{
SubmeshDirectedEdgeId(self.0|((parity as u32)<<(u32::BITS-1)))
}
}
impl DirectedEdge for SubmeshDirectedEdgeId{
type UndirectedEdge=SubmeshEdgeId;
fn as_undirected(&self)->SubmeshEdgeId{
fn as_undirected(self)->SubmeshEdgeId{
SubmeshEdgeId(self.0&!(1<<(u32::BITS-1)))
}
fn parity(&self)->bool{
@@ -59,14 +43,6 @@ impl DirectedEdge for SubmeshDirectedEdgeId{
}
}
//Vertex <-> Edge <-> Face -> Collide
#[derive(Debug)]
pub enum FEV<M:MeshQuery>{
Face(M::Face),
Edge(<M::Edge as DirectedEdge>::UndirectedEdge),
Vert(M::Vert),
}
//use Unit32 #[repr(C)] for map files
#[derive(Clone,Copy,Debug,Hash,Eq,PartialEq)]
struct Face{
@@ -75,32 +51,6 @@ struct Face{
}
#[derive(Debug)]
struct Vert(Planar64Vec3);
pub trait MeshQuery{
type Face:Copy;
type Edge:Copy+DirectedEdge;
type Vert:Copy;
// Vertex must be Planar64Vec3 because it represents an actual position
type Normal;
type Offset;
fn edge_n(&self,edge_id:<Self::Edge as DirectedEdge>::UndirectedEdge)->Planar64Vec3{
let &[v0,v1]=self.edge_verts(edge_id).as_ref();
self.vert(v1)-self.vert(v0)
}
fn directed_edge_n(&self,directed_edge_id:Self::Edge)->Planar64Vec3{
let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
(self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1)
}
/// This must return a point inside the mesh.
#[expect(dead_code)]
fn hint_point(&self)->Planar64Vec3;
fn vert(&self,vert_id:Self::Vert)->Planar64Vec3;
fn face_nd(&self,face_id:Self::Face)->(Self::Normal,Self::Offset);
fn face_edges(&self,face_id:Self::Face)->impl AsRef<[Self::Edge]>;
fn edge_faces(&self,edge_id:<Self::Edge as DirectedEdge>::UndirectedEdge)->impl AsRef<[Self::Face;2]>;
fn edge_verts(&self,edge_id:<Self::Edge as DirectedEdge>::UndirectedEdge)->impl AsRef<[Self::Vert;2]>;
fn vert_edges(&self,vert_id:Self::Vert)->impl AsRef<[Self::Edge]>;
fn vert_faces(&self,vert_id:Self::Vert)->impl AsRef<[Self::Face]>;
}
#[derive(Debug)]
struct FaceRefs{
// I didn't write it down, but I assume the edges are directed
@@ -439,15 +389,14 @@ impl TryFrom<&model::Mesh> for PhysicsMesh{
}
}
#[derive(Debug)]
#[derive(Debug,Clone,Copy)]
pub struct PhysicsMeshView<'a>{
data:&'a PhysicsMeshData,
topology:&'a PhysicsMeshTopology,
}
impl MeshQuery for PhysicsMeshView<'_>{
type Face=SubmeshFaceId;
type Edge=SubmeshDirectedEdgeId;
type Vert=SubmeshVertId;
type Position=Planar64Vec3;
type Direction=Planar64Vec3;
type Normal=Planar64Vec3;
type Offset=Planar64;
fn face_nd(&self,face_id:SubmeshFaceId)->(Planar64Vec3,Planar64){
@@ -458,25 +407,54 @@ impl MeshQuery for PhysicsMeshView<'_>{
// invariant: meshes always encompass the origin
vec3::zero()
}
fn farthest_vert(&self,dir:Planar64Vec3)->SubmeshVertId{
//this happens to be well-defined. there are no virtual virtices
SubmeshVertId::new(
self.topology.verts.iter()
.enumerate()
.max_by_key(|&(_,&vert_id)|
dir.dot(self.data.verts[vert_id.get() as usize].0)
)
//assume there is more than zero vertices.
.unwrap().0 as u32
)
}
//ideally I never calculate the vertex position, but I have to for the graphical meshes...
fn vert(&self,vert_id:SubmeshVertId)->Planar64Vec3{
let vert_idx=self.topology.verts[vert_id.get() as usize].get() as usize;
self.data.verts[vert_idx].0
}
fn face_edges(&self,face_id:SubmeshFaceId)->impl AsRef<[SubmeshDirectedEdgeId]>{
self.topology.face_topology[face_id.get() as usize].edges.as_slice()
fn edge_n(&self,edge_id:Self::Edge)->Self::Direction{
let &[v0,v1]=self.edge_verts(edge_id).as_ref();
self.vert(v1)-self.vert(v0)
}
fn edge_faces(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshFaceId;2]>{
fn directed_edge_n(&self,directed_edge_id:Self::DirectedEdge)->Self::Direction{
let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
(self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1)
}
}
impl MeshTopology for PhysicsMeshView<'_>{
type Face=SubmeshFaceId;
type Edge=SubmeshEdgeId;
type DirectedEdge=SubmeshDirectedEdgeId;
type Vert=SubmeshVertId;
fn for_each_vert_edge(&self,vert_id:Self::Vert,f:impl FnMut(Self::DirectedEdge)){
self.topology.vert_topology[vert_id.get() as usize].edges.iter().copied().for_each(f);
}
fn for_each_vert_face(&self,vert_id:Self::Vert,f:impl FnMut(Self::Face)){
self.topology.vert_topology[vert_id.get() as usize].faces.iter().copied().for_each(f);
}
fn edge_faces(&self,edge_id:Self::Edge)->impl AsRef<[Self::Face;2]>{
AsRefHelper(self.topology.edge_topology[edge_id.get() as usize].faces)
}
fn edge_verts(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshVertId;2]>{
fn edge_verts(&self,edge_id:Self::Edge)->impl AsRef<[Self::Vert;2]>{
AsRefHelper(self.topology.edge_topology[edge_id.get() as usize].verts)
}
fn vert_edges(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshDirectedEdgeId]>{
self.topology.vert_topology[vert_id.get() as usize].edges.as_slice()
fn for_each_face_vert(&self,_face_id:Self::Face,_f:impl FnMut(Self::Vert)){
unimplemented!()
}
fn vert_faces(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshFaceId]>{
self.topology.vert_topology[vert_id.get() as usize].faces.as_slice()
fn for_each_face_edge(&self,face_id:Self::Face,f:impl FnMut(Self::DirectedEdge)){
self.topology.face_topology[face_id.get() as usize].edges.iter().copied().for_each(f);
}
}
@@ -496,7 +474,7 @@ impl PhysicsMeshTransform{
}
}
#[derive(Debug)]
#[derive(Debug,Clone,Copy)]
pub struct TransformedMesh<'a>{
view:PhysicsMeshView<'a>,
transform:&'a PhysicsMeshTransform,
@@ -514,23 +492,10 @@ impl TransformedMesh<'_>{
pub fn verts<'a>(&'a self)->impl Iterator<Item=Vector3<Fixed<2,64>>>+'a{
self.view.data.verts.iter().map(|&Vert(pos)|self.transform.vertex.transform_point3(pos))
}
fn farthest_vert(&self,dir:Planar64Vec3)->SubmeshVertId{
//this happens to be well-defined. there are no virtual virtices
SubmeshVertId::new(
self.view.topology.verts.iter()
.enumerate()
.max_by_key(|&(_,&vert_id)|
dir.dot(self.transform.vertex.transform_point3(self.view.data.verts[vert_id.get() as usize].0))
)
//assume there is more than zero vertices.
.unwrap().0 as u32
)
}
}
impl MeshQuery for TransformedMesh<'_>{
type Face=SubmeshFaceId;
type Edge=SubmeshDirectedEdgeId;
type Vert=SubmeshVertId;
type Direction=Planar64Vec3;
type Position=Planar64Vec3;
type Normal=Vector3<Fixed<3,96>>;
type Offset=Fixed<4,128>;
fn face_nd(&self,face_id:SubmeshFaceId)->(Self::Normal,Self::Offset){
@@ -546,510 +511,59 @@ impl MeshQuery for TransformedMesh<'_>{
fn hint_point(&self)->Planar64Vec3{
self.transform.vertex.translation
}
fn farthest_vert(&self,dir:Planar64Vec3)->SubmeshVertId{
//this happens to be well-defined. there are no virtual virtices
SubmeshVertId::new(
self.view.topology.verts.iter()
.enumerate()
.max_by_key(|&(_,&vert_id)|
dir.dot(self.transform.vertex.transform_point3(self.view.data.verts[vert_id.get() as usize].0))
)
//assume there is more than zero vertices.
.unwrap().0 as u32
)
}
fn edge_n(&self,edge_id:Self::Edge)->Self::Direction{
let &[v0,v1]=self.edge_verts(edge_id).as_ref();
self.vert(v1)-self.vert(v0)
}
fn directed_edge_n(&self,directed_edge_id:Self::DirectedEdge)->Self::Direction{
let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
(self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1)
}
}
impl MeshTopology for TransformedMesh<'_>{
type Face=SubmeshFaceId;
type Edge=SubmeshEdgeId;
type DirectedEdge=SubmeshDirectedEdgeId;
type Vert=SubmeshVertId;
#[inline]
fn face_edges(&self,face_id:SubmeshFaceId)->impl AsRef<[SubmeshDirectedEdgeId]>{
self.view.face_edges(face_id)
fn for_each_vert_edge(&self,vert_id:Self::Vert,f:impl FnMut(Self::DirectedEdge)){
self.view.for_each_vert_edge(vert_id,f)
}
#[inline]
fn edge_faces(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshFaceId;2]>{
fn for_each_vert_face(&self,vert_id:Self::Vert,f:impl FnMut(Self::Face)){
self.view.for_each_vert_face(vert_id,f)
}
#[inline]
fn edge_faces(&self,edge_id:Self::Edge)->impl AsRef<[Self::Face;2]>{
self.view.edge_faces(edge_id)
}
#[inline]
fn edge_verts(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshVertId;2]>{
fn edge_verts(&self,edge_id:Self::Edge)->impl AsRef<[Self::Vert;2]>{
self.view.edge_verts(edge_id)
}
#[inline]
fn vert_edges(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshDirectedEdgeId]>{
self.view.vert_edges(vert_id)
fn for_each_face_vert(&self,face_id:Self::Face,f:impl FnMut(Self::Vert)){
self.view.for_each_face_vert(face_id,f)
}
#[inline]
fn vert_faces(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshFaceId]>{
self.view.vert_faces(vert_id)
fn for_each_face_edge(&self,face_id:Self::Face,f:impl FnMut(Self::DirectedEdge)){
self.view.for_each_face_edge(face_id,f)
}
}
//Note that a face on a minkowski mesh refers to a pair of fevs on the meshes it's summed from
//(face,vertex)
//(edge,edge)
//(vertex,face)
#[derive(Clone,Copy,Debug)]
pub enum MinkowskiVert{
VertVert(SubmeshVertId,SubmeshVertId),
}
#[derive(Clone,Copy,Debug)]
pub enum MinkowskiEdge{
VertEdge(SubmeshVertId,SubmeshEdgeId),
EdgeVert(SubmeshEdgeId,SubmeshVertId),
//EdgeEdge when edges are parallel
}
impl UndirectedEdge for MinkowskiEdge{
type DirectedEdge=MinkowskiDirectedEdge;
fn as_directed(&self,parity:bool)->Self::DirectedEdge{
match self{
MinkowskiEdge::VertEdge(v0,e1)=>MinkowskiDirectedEdge::VertEdge(*v0,e1.as_directed(parity)),
MinkowskiEdge::EdgeVert(e0,v1)=>MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),*v1),
}
}
}
#[derive(Clone,Copy,Debug)]
pub enum MinkowskiDirectedEdge{
VertEdge(SubmeshVertId,SubmeshDirectedEdgeId),
EdgeVert(SubmeshDirectedEdgeId,SubmeshVertId),
//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(Clone,Copy,Debug,Hash,Eq,PartialEq)]
pub enum MinkowskiFace{
VertFace(SubmeshVertId,SubmeshFaceId),
EdgeEdge(SubmeshEdgeId,SubmeshEdgeId,bool),
FaceVert(SubmeshFaceId,SubmeshVertId),
//EdgeFace
//FaceEdge
//FaceFace
}
#[derive(Debug)]
pub struct MinkowskiMesh<'a>{
mesh0:TransformedMesh<'a>,
mesh1:TransformedMesh<'a>,
}
//infinity fev algorithm state transition
#[derive(Debug)]
enum Transition{
Done,//found closest vert, no edges are better
Vert(MinkowskiVert),//transition to vert
}
enum EV{
Vert(MinkowskiVert),
Edge(MinkowskiEdge),
}
pub type GigaTime=Ratio<Fixed<4,128>,Fixed<4,128>>;
pub fn into_giga_time(time:Time,relative_to:Time)->GigaTime{
let r=(time-relative_to).to_ratio();
Ratio::new(r.num.widen_4(),r.den.widen_4())
}
impl MinkowskiMesh<'_>{
pub fn minkowski_sum<'a>(mesh0:TransformedMesh<'a>,mesh1:TransformedMesh<'a>)->MinkowskiMesh<'a>{
MinkowskiMesh{
mesh0,
mesh1,
}
}
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 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).as_ref(){
let edge_n=self.directed_edge_n(directed_edge_id);
//is boundary uncrossable by a crawl from infinity
let edge_verts=self.edge_verts(directed_edge_id.as_undirected());
//select opposite vertex
let test_vert_id=edge_verts.as_ref()[directed_edge_id.parity() as usize];
//test if it's closer
let diff=point-self.vert(test_vert_id);
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);
*best_distance_squared=distance_squared;
}
}
}
best_transition
}
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).as_ref(){
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
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 !d.is_negative()&&d<=edge_nn{
let distance_squared={
let c=diff.cross(edge_n);
//wrap for speed
(c.dot(c)/edge_nn).divide().wrap_2()
};
if distance_squared<=*best_distance_squared{
best_transition=EV::Edge(directed_edge_id.as_undirected());
*best_distance_squared=distance_squared;
}
}
}
}
best_transition
}
fn crawl_boundaries(&self,mut vert_id:MinkowskiVert,infinity_dir:Planar64Vec3,point:Planar64Vec3)->EV{
let mut best_distance_squared={
let diff=point-self.vert(vert_id);
diff.dot(diff)
};
loop{
match self.next_transition_vert(vert_id,&mut best_distance_squared,infinity_dir,point){
Transition::Done=>return self.final_ev(vert_id,&mut best_distance_squared,infinity_dir,point),
Transition::Vert(new_vert_id)=>vert_id=new_vert_id,
}
}
}
/// 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::<MinkowskiMesh<'_>>{
//start on any vertex
//cross uncrossable vertex-edge boundaries until you find the closest vertex or edge
//cross edge-face boundary if it's uncrossable
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
EV::Vert(vert_id)=>FEV::Vert(vert_id),
EV::Edge(edge_id)=>{
//cross to face if the boundary is not crossable and we are on the wrong side
let edge_n=self.edge_n(edge_id);
// point is multiplied by two because vert_sum sums two vertices.
let delta_pos=point*2-{
let &[v0,v1]=self.edge_verts(edge_id).as_ref();
self.vert(v0)+self.vert(v1)
};
for (i,&face_id) in self.edge_faces(edge_id).as_ref().iter().enumerate(){
let face_n=self.face_nd(face_id).0;
//edge-face boundary nd, n facing out of the face towards the edge
let boundary_n=face_n.cross(edge_n)*(i as i64*2-1);
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.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);
}
}
FEV::Edge(edge_id)
},
}
}
// TODO: fundamentally improve this algorithm.
// All it needs to do is find the closest point on the mesh
// and return the FEV which the point resides on.
//
// What it actually does is use the above functions to trace a ray in from infinity,
// crawling the closest point along the mesh surface until the ray reaches
// the starting point to discover the final FEV.
//
// The actual collision prediction probably does a single test
// and then immediately returns with 0 FEV transitions on average,
// because of the strict time_limit constraint.
//
// Most of the calculation time is just calculating the starting point
// for the "actual" crawling algorithm below (predict_collision_{in|out}).
fn closest_fev_not_inside(&self,mut infinity_body:Body,start_time:Bound<&Time>)->Option<FEV<MinkowskiMesh<'_>>>{
infinity_body.infinity_dir().and_then(|dir|{
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=vec3::zero();
//crawl in from negative infinity along a tangent line to get the closest fev
infinity_fev.crawl(self,&infinity_body,Bound::Unbounded,start_time).miss()
})
}
pub fn predict_collision_in(&self,relative_body:&Body,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
self.closest_fev_not_inside(*relative_body,range.start_bound()).and_then(|fev|{
//continue forwards along the body parabola
fev.crawl(self,relative_body,range.start_bound(),range.end_bound()).hit()
})
}
pub fn predict_collision_out(&self,relative_body:&Body,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
let (lower_bound,upper_bound)=(range.start_bound(),range.end_bound());
// swap and negate bounds to do a time inversion
let (lower_bound,upper_bound)=(upper_bound.map(|&t|-t),lower_bound.map(|&t|-t));
let infinity_body=-relative_body;
self.closest_fev_not_inside(infinity_body,lower_bound.as_ref()).and_then(|fev|{
//continue backwards along the body parabola
fev.crawl(self,&infinity_body,lower_bound.as_ref(),upper_bound.as_ref()).hit()
//no need to test -time<time_limit because of the first step
.map(|(face,time)|(face,-time))
})
}
pub fn predict_collision_face_out(&self,relative_body:&Body,range:impl RangeBounds<Time>,contact_face_id:MinkowskiFace)->Option<(MinkowskiDirectedEdge,GigaTime)>{
// TODO: make better
use crate::face_crawler::{low,upp};
//no algorithm needed, there is only one state and two cases (Edge,None)
//determine when it passes an edge ("sliding off" case)
let start_time=range.start_bound().map(|&t|(t-relative_body.time).to_ratio());
let mut best_time=range.end_bound().map(|&t|into_giga_time(t,relative_body.time));
let mut best_edge=None;
let face_n=self.face_nd(contact_face_id).0;
for &directed_edge_id in self.face_edges(contact_face_id).as_ref(){
let edge_n=self.directed_edge_n(directed_edge_id);
//f x e points in
let n=face_n.cross(edge_n);
let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
let d=n.dot(self.vert(v0)+self.vert(v1));
//WARNING! d outside of *2
//WARNING: truncated precision
//wrap for speed
for dt in Fixed::<4,128>::zeroes2(((n.dot(relative_body.position))*2-d).wrap_4(),n.dot(relative_body.velocity).wrap_4()*2,n.dot(relative_body.acceleration).wrap_4()){
if low(&start_time,&dt)&&upp(&dt,&best_time)&&n.dot(relative_body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
best_time=Bound::Included(dt);
best_edge=Some((directed_edge_id,dt));
break;
}
}
}
best_edge
}
fn infinity_in(&self,infinity_body:Body)->Option<(MinkowskiFace,GigaTime)>{
let infinity_fev=self.infinity_fev(-infinity_body.velocity,infinity_body.position);
// Bound::Included means that the surface of the mesh is included in the mesh
infinity_fev.crawl(self,&infinity_body,Bound::Unbounded,Bound::Included(&infinity_body.time)).hit()
}
pub fn is_point_in_mesh(&self,point:Planar64Vec3)->bool{
let infinity_body=Body::new(point,vec3::Y,vec3::zero(),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)
.is_some_and(|_|
self.infinity_in(-infinity_body)
.is_some()
)
}
}
impl MeshQuery for MinkowskiMesh<'_>{
type Face=MinkowskiFace;
type Edge=MinkowskiDirectedEdge;
type Vert=MinkowskiVert;
type Normal=Vector3<Fixed<3,96>>;
type Offset=Fixed<4,128>;
// TODO: relative d
fn face_nd(&self,face_id:MinkowskiFace)->(Self::Normal,Self::Offset){
match face_id{
MinkowskiFace::VertFace(v0,f1)=>{
let (n,d)=self.mesh1.face_nd(f1);
(-n,d-n.dot(self.mesh0.vert(v0)))
},
MinkowskiFace::EdgeEdge(e0,e1,parity)=>{
let edge0_n=self.mesh0.edge_n(e0);
let edge1_n=self.mesh1.edge_n(e1);
let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).as_ref();
let &[e1v0,e1v1]=self.mesh1.edge_verts(e1).as_ref();
let n=edge0_n.cross(edge1_n);
let e0d=n.dot(self.mesh0.vert(e0v0)+self.mesh0.vert(e0v1));
let e1d=n.dot(self.mesh1.vert(e1v0)+self.mesh1.vert(e1v1));
((n*(parity as i64*4-2)).widen_3(),((e0d-e1d)*(parity as i64*2-1)).widen_4())
},
MinkowskiFace::FaceVert(f0,v1)=>{
let (n,d)=self.mesh0.face_nd(f0);
(n,d-n.dot(self.mesh1.vert(v1)))
},
}
}
fn vert(&self,vert_id:MinkowskiVert)->Planar64Vec3{
match vert_id{
MinkowskiVert::VertVert(v0,v1)=>{
self.mesh0.vert(v0)-self.mesh1.vert(v1)
},
}
}
fn hint_point(&self)->Planar64Vec3{
self.mesh1.transform.vertex.translation-
self.mesh0.transform.vertex.translation
}
fn face_edges(&self,face_id:MinkowskiFace)->impl AsRef<[MinkowskiDirectedEdge]>{
match face_id{
MinkowskiFace::VertFace(v0,f1)=>{
self.mesh1.face_edges(f1).as_ref().iter().map(|&edge_id1|
MinkowskiDirectedEdge::VertEdge(v0,edge_id1.reverse())
).collect()
},
MinkowskiFace::EdgeEdge(e0,e1,parity)=>{
let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).as_ref();
let &[e1v0,e1v1]=self.mesh1.edge_verts(e1).as_ref();
//could sort this if ordered edges are needed
//probably just need to reverse this list according to parity
vec![
MinkowskiDirectedEdge::VertEdge(e0v0,e1.as_directed(parity)),
MinkowskiDirectedEdge::EdgeVert(e0.as_directed(!parity),e1v0),
MinkowskiDirectedEdge::VertEdge(e0v1,e1.as_directed(!parity)),
MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),e1v1),
]
},
MinkowskiFace::FaceVert(f0,v1)=>{
self.mesh0.face_edges(f0).as_ref().iter().map(|&edge_id0|
MinkowskiDirectedEdge::EdgeVert(edge_id0,v1)
).collect()
},
}
}
fn edge_faces(&self,edge_id:MinkowskiEdge)->impl AsRef<[MinkowskiFace;2]>{
match edge_id{
MinkowskiEdge::VertEdge(v0,e1)=>{
//faces are listed backwards from the minkowski mesh
let v0e=self.mesh0.vert_edges(v0);
let &[e1f0,e1f1]=self.mesh1.edge_faces(e1).as_ref();
AsRefHelper([(e1f1,false),(e1f0,true)].map(|(edge_face_id1,face_parity)|{
let mut best_edge=None;
let mut best_d:Ratio<Fixed<8,256>,Fixed<8,256>>=Ratio::new(Fixed::ZERO,Fixed::ONE);
let edge_face1_n=self.mesh1.face_nd(edge_face_id1).0;
let edge_face1_nn=edge_face1_n.dot(edge_face1_n);
for &directed_edge_id0 in v0e.as_ref(){
let edge0_n=self.mesh0.directed_edge_n(directed_edge_id0);
//must be behind other face.
let d=edge_face1_n.dot(edge0_n);
if d.is_negative(){
let edge0_nn=edge0_n.dot(edge0_n);
// Assume not every number is huge
// TODO: revisit this
let dd=(d*d)/(edge_face1_nn*edge0_nn);
if best_d<dd{
best_d=dd;
best_edge=Some(directed_edge_id0);
}
}
}
best_edge.map_or(
MinkowskiFace::VertFace(v0,edge_face_id1),
|directed_edge_id0|MinkowskiFace::EdgeEdge(directed_edge_id0.as_undirected(),e1,directed_edge_id0.parity()^face_parity)
)
}))
},
MinkowskiEdge::EdgeVert(e0,v1)=>{
//tracking index with an external variable because .enumerate() is not available
let v1e=self.mesh1.vert_edges(v1);
let &[e0f0,e0f1]=self.mesh0.edge_faces(e0).as_ref();
AsRefHelper([(e0f0,true),(e0f1,false)].map(|(edge_face_id0,face_parity)|{
let mut best_edge=None;
let mut best_d:Ratio<Fixed<8,256>,Fixed<8,256>>=Ratio::new(Fixed::ZERO,Fixed::ONE);
let edge_face0_n=self.mesh0.face_nd(edge_face_id0).0;
let edge_face0_nn=edge_face0_n.dot(edge_face0_n);
for &directed_edge_id1 in v1e.as_ref(){
let edge1_n=self.mesh1.directed_edge_n(directed_edge_id1);
let d=edge_face0_n.dot(edge1_n);
if d.is_negative(){
let edge1_nn=edge1_n.dot(edge1_n);
let dd=(d*d)/(edge_face0_nn*edge1_nn);
if best_d<dd{
best_d=dd;
best_edge=Some(directed_edge_id1);
}
}
}
best_edge.map_or(
MinkowskiFace::FaceVert(edge_face_id0,v1),
|directed_edge_id1|MinkowskiFace::EdgeEdge(e0,directed_edge_id1.as_undirected(),directed_edge_id1.parity()^face_parity)
)
}))
},
}
}
fn edge_verts(&self,edge_id:MinkowskiEdge)->impl AsRef<[MinkowskiVert;2]>{
AsRefHelper(match edge_id{
MinkowskiEdge::VertEdge(v0,e1)=>self.mesh1.edge_verts(e1).as_ref().map(|vert_id1|
MinkowskiVert::VertVert(v0,vert_id1)
),
MinkowskiEdge::EdgeVert(e0,v1)=>self.mesh0.edge_verts(e0).as_ref().map(|vert_id0|
MinkowskiVert::VertVert(vert_id0,v1)
),
})
}
fn vert_edges(&self,vert_id:MinkowskiVert)->impl AsRef<[MinkowskiDirectedEdge]>{
match vert_id{
MinkowskiVert::VertVert(v0,v1)=>{
let mut edges=Vec::new();
//detect shared volume when the other mesh is mirrored along a test edge dir
let v0f_thing=self.mesh0.vert_faces(v0);
let v1f_thing=self.mesh1.vert_faces(v1);
let v0f=v0f_thing.as_ref();
let v1f=v1f_thing.as_ref();
let v0f_n:Vec<_>=v0f.iter().map(|&face_id|self.mesh0.face_nd(face_id).0).collect();
let v1f_n:Vec<_>=v1f.iter().map(|&face_id|self.mesh1.face_nd(face_id).0).collect();
// scratch vector
let mut face_normals=Vec::with_capacity(v0f.len()+v1f.len());
face_normals.clone_from(&v0f_n);
for &directed_edge_id in self.mesh0.vert_edges(v0).as_ref(){
let n=self.mesh0.directed_edge_n(directed_edge_id);
let nn=n.dot(n);
// TODO: there's gotta be a better way to do this
// drop faces beyond v0f_n
face_normals.truncate(v0f.len());
// make a set of faces from mesh0's perspective
for face_n in &v1f_n{
//add reflected mesh1 faces
//wrap for speed
face_normals.push(*face_n-(n*face_n.dot(n)*2/nn).divide().wrap_3());
}
if is_empty_volume(&face_normals){
edges.push(MinkowskiDirectedEdge::EdgeVert(directed_edge_id,v1));
}
}
face_normals.clone_from(&v1f_n);
for &directed_edge_id in self.mesh1.vert_edges(v1).as_ref(){
let n=self.mesh1.directed_edge_n(directed_edge_id);
let nn=n.dot(n);
// drop faces beyond v1f_n
face_normals.truncate(v1f.len());
// make a set of faces from mesh1's perspective
for face_n in &v0f_n{
//wrap for speed
face_normals.push(*face_n-(n*face_n.dot(n)*2/nn).divide().wrap_3());
}
if is_empty_volume(&face_normals){
edges.push(MinkowskiDirectedEdge::VertEdge(v0,directed_edge_id));
}
}
edges
},
}
}
fn vert_faces(&self,_vert_id:MinkowskiVert)->impl AsRef<[MinkowskiFace]>{
unimplemented!();
#[expect(unreachable_code)]
Vec::new()
}
}
fn is_empty_volume(normals:&[Vector3<Fixed<3,96>>])->bool{
let len=normals.len();
for i in 0..len-1{
for j in i+1..len{
let n=normals[i].cross(normals[j]);
let mut d_comp=None;
for k in 0..len{
if k!=i&&k!=j{
let d=n.dot(normals[k]).is_negative();
if let &Some(comp)=&d_comp{
// This is testing if d_comp*d < 0
if comp^d{
return true;
}
}else{
d_comp=Some(d);
}
}
}
}
}
return false;
}
#[test]
fn test_is_empty_volume(){
assert!(!is_empty_volume(&[vec3::X.widen_3(),vec3::Y.widen_3(),vec3::Z.widen_3()]));
assert!(is_empty_volume(&[vec3::X.widen_3(),vec3::Y.widen_3(),vec3::Z.widen_3(),vec3::NEG_X.widen_3()]));
}

View File

@@ -1,5 +1,7 @@
use std::collections::{HashMap,HashSet};
use crate::model::{self as model_physics,PhysicsMesh,PhysicsMeshTransform,TransformedMesh,MeshQuery,PhysicsMeshId,PhysicsSubmeshId};
use crate::mesh_query::MeshQuery;
use crate::minkowski::{MinkowskiMesh,MinkowskiFace};
use crate::model::{self as model_physics,PhysicsMesh,PhysicsMeshTransform,TransformedMesh,PhysicsMeshId,PhysicsSubmeshId};
use strafesnet_common::bvh;
use strafesnet_common::map;
use strafesnet_common::run;
@@ -15,6 +17,7 @@ pub use strafesnet_common::physics::{Time,TimeInner};
use gameplay::ModeState;
pub type Body=crate::body::Body<TimeInner>;
pub type Trajectory=crate::body::Trajectory<TimeInner>;
type MouseState=strafesnet_common::mouse::MouseState<TimeInner>;
//external influence
@@ -28,6 +31,7 @@ pub enum InternalInstruction{
CollisionStart(Collision,model_physics::GigaTime),
CollisionEnd(Collision,model_physics::GigaTime),
StrafeTick,
// TODO: add GigaTime to ReachWalkTargetVelocity
ReachWalkTargetVelocity,
// Water,
}
@@ -53,6 +57,9 @@ impl InputState{
fn replace_mouse(&mut self,mouse:MouseState,next_mouse:MouseState){
(self.next_mouse,self.mouse)=(next_mouse,mouse);
}
fn get_control(&self,control:Controls)->bool{
self.controls.contains(control)
}
fn set_control(&mut self,control:Controls,state:bool){
self.controls.set(control,state)
}
@@ -498,20 +505,24 @@ enum MoveState{
}
impl MoveState{
//call this after state.move_state is changed
fn apply_enum(&self,body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
fn acceleration(&self,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState)->Planar64Vec3{
match self{
MoveState::Fly=>body.acceleration=vec3::zero(),
MoveState::Air=>{
//calculate base acceleration
let a=touching.base_acceleration(models,style,camera,input_state);
//set_acceleration clips according to contacts
set_acceleration(body,touching,models,hitbox_mesh,a);
MoveState::Fly=>vec3::zero(),
MoveState::Air
|MoveState::Water
=>{
// calculate base acceleration
let base_acceleration=touching.base_acceleration(models,style,camera,input_state);
// constrain_acceleration clips according to contacts
touching.constrain_acceleration(models,hitbox_mesh,base_acceleration)
},
_=>(),
MoveState::Walk(walk_state)
|MoveState::Ladder(walk_state)
=>walk_state.target.acceleration(),
}
}
//function to coerce &mut self into &self
fn apply_to_body(&self,body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
fn update_fly_velocity(&self,body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
match self{
MoveState::Air=>(),
MoveState::Water=>(),
@@ -521,17 +532,13 @@ impl MoveState{
//set_velocity clips velocity according to current touching state
set_velocity(body,touching,models,hitbox_mesh,v);
},
MoveState::Walk(walk_state)
|MoveState::Ladder(walk_state)
=>{
//accelerate towards walk target or do nothing
let a=walk_state.target.acceleration();
set_acceleration(body,touching,models,hitbox_mesh,a);
},
MoveState::Walk(_walk_state)
|MoveState::Ladder(_walk_state)
=>(),
}
}
/// changes the move state
fn apply_input(&mut self,body:&Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
fn update_walk_target(&mut self,body:&Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
match self{
MoveState::Fly
|MoveState::Air
@@ -588,24 +595,11 @@ impl MoveState{
MoveState::Fly=>None,
}
}
//lmao idk this is convenient
fn apply_enum_and_input_and_body(&mut self,body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
self.apply_enum(body,touching,models,hitbox_mesh,style,camera,input_state);
self.apply_input(body,touching,models,hitbox_mesh,style,camera,input_state);
self.apply_to_body(body,touching,models,hitbox_mesh,style,camera,input_state);
}
fn apply_enum_and_body(&mut self,body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
self.apply_enum(body,touching,models,hitbox_mesh,style,camera,input_state);
self.apply_to_body(body,touching,models,hitbox_mesh,style,camera,input_state);
}
fn apply_input_and_body(&mut self,body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
self.apply_input(body,touching,models,hitbox_mesh,style,camera,input_state);
self.apply_to_body(body,touching,models,hitbox_mesh,style,camera,input_state);
}
fn set_move_state(&mut self,move_state:MoveState,body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
*self=move_state;
//this function call reads the above state that was just set
self.apply_enum_and_body(body,touching,models,hitbox_mesh,style,camera,input_state);
self.update_walk_target(body,touching,models,hitbox_mesh,style,camera,input_state);
self.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state);
}
fn cull_velocity(&mut self,velocity:Planar64Vec3,body:&mut Body,touching:&mut TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,style:&StyleModifiers,camera:&PhysicsCamera,input_state:&InputState){
//TODO: be more precise about contacts
@@ -618,10 +612,11 @@ impl MoveState{
self.set_move_state(MoveState::Air,body,touching,models,hitbox_mesh,style,camera,input_state);
}else{
// stopped touching something else while walking
self.apply_enum_and_input_and_body(body,touching,models,hitbox_mesh,style,camera,input_state);
self.update_walk_target(body,touching,models,hitbox_mesh,style,camera,input_state);
self.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state);
},
// not walking, but stopped touching something
None=>self.apply_enum_and_body(body,touching,models,hitbox_mesh,style,camera,input_state),
None=>self.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state),
}
}
}
@@ -729,22 +724,22 @@ struct IntersectModel{
transform:PhysicsMeshTransform,
}
#[derive(Debug,Clone,Copy,Eq,Hash,PartialEq)]
#[derive(Debug,Clone,Copy,Hash)]
pub struct ContactCollision{
convex_mesh_id:ConvexMeshId<ContactModelId>,
face_id:model_physics::MinkowskiFace,
face_id:MinkowskiFace,
}
#[derive(Debug,Clone,Copy,Eq,Hash,PartialEq)]
pub struct IntersectCollision{
convex_mesh_id:ConvexMeshId<IntersectModelId>,
}
#[derive(Debug,Clone,Eq,Hash,PartialEq)]
#[derive(Debug,Clone,Hash)]
pub enum Collision{
Contact(ContactCollision),
Intersect(IntersectCollision),
}
impl Collision{
fn new(convex_mesh_id:ConvexMeshId<PhysicsModelId>,face_id:model_physics::MinkowskiFace)->Self{
fn new(convex_mesh_id:ConvexMeshId<PhysicsModelId>,face_id:MinkowskiFace)->Self{
match convex_mesh_id.model_id{
PhysicsModelId::Contact(model_id)=>Collision::Contact(ContactCollision{convex_mesh_id:convex_mesh_id.map(model_id),face_id}),
PhysicsModelId::Intersect(model_id)=>Collision::Intersect(IntersectCollision{convex_mesh_id:convex_mesh_id.map(model_id)}),
@@ -755,7 +750,7 @@ impl Collision{
struct TouchingState{
// This is kind of jank, it's a ContactCollision
// but split over the Key and Value of the HashMap.
contacts:HashMap<ConvexMeshId<ContactModelId>,model_physics::MinkowskiFace>,
contacts:HashMap<ConvexMeshId<ContactModelId>,MinkowskiFace>,
intersects:HashSet<ConvexMeshId<IntersectModelId>>,
}
impl TouchingState{
@@ -763,13 +758,13 @@ impl TouchingState{
self.contacts.clear();
self.intersects.clear();
}
fn insert_contact(&mut self,contact:ContactCollision)->Option<model_physics::MinkowskiFace>{
fn insert_contact(&mut self,contact:ContactCollision)->Option<MinkowskiFace>{
self.contacts.insert(contact.convex_mesh_id,contact.face_id)
}
fn insert_intersect(&mut self,intersect:IntersectCollision)->bool{
self.intersects.insert(intersect.convex_mesh_id)
}
fn remove_contact(&mut self,convex_mesh_id:&ConvexMeshId<ContactModelId>)->Option<model_physics::MinkowskiFace>{
fn remove_contact(&mut self,convex_mesh_id:&ConvexMeshId<ContactModelId>)->Option<MinkowskiFace>{
self.contacts.remove(convex_mesh_id)
}
fn remove_intersect(&mut self,convex_mesh_id:&ConvexMeshId<IntersectModelId>)->bool{
@@ -815,7 +810,7 @@ impl TouchingState{
normal:n,
}
}).collect();
crate::push_solve::push_solve(&contacts,velocity)
crate::push_solve::push_solve(&contacts,velocity).0
}
fn constrain_acceleration(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,acceleration:Planar64Vec3)->Planar64Vec3{
let contacts:Vec<_>=self.contacts.iter().map(|(convex_mesh_id,face_id)|{
@@ -826,18 +821,17 @@ impl TouchingState{
normal:n,
}
}).collect();
crate::push_solve::push_solve(&contacts,acceleration)
crate::push_solve::push_solve(&contacts,acceleration).0
}
fn predict_collision_end(&self,collector:&mut instruction::InstructionCollector<InternalInstruction,Time>,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,body:&Body,start_time:Time){
fn predict_collision_end(&self,collector:&mut instruction::InstructionCollector<InternalInstruction,Time>,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,trajectory:&Trajectory,start_time:Time){
// let relative_body=body.relative_to(&Body::ZERO);
let relative_body=body;
for (convex_mesh_id,face_id) in &self.contacts{
//detect face slide off
let model_mesh=models.contact_mesh(convex_mesh_id);
let minkowski=model_physics::MinkowskiMesh::minkowski_sum(model_mesh,hitbox_mesh.transformed_mesh());
collector.collect(minkowski.predict_collision_face_out(&relative_body,start_time..collector.time(),*face_id).map(|(_face,time)|{
let minkowski=MinkowskiMesh::minkowski_sum(model_mesh,hitbox_mesh.transformed_mesh());
collector.collect(minkowski.predict_collision_face_out(&trajectory,start_time..collector.time(),*face_id).map(|(_face,time)|{
TimedInstruction{
time:relative_body.time+time.into(),
time:trajectory.time+time.into(),
instruction:InternalInstruction::CollisionEnd(
Collision::Contact(ContactCollision{face_id:*face_id,convex_mesh_id:*convex_mesh_id}),
time
@@ -848,10 +842,10 @@ impl TouchingState{
for convex_mesh_id in &self.intersects{
//detect model collision in reverse
let model_mesh=models.intersect_mesh(convex_mesh_id);
let minkowski=model_physics::MinkowskiMesh::minkowski_sum(model_mesh,hitbox_mesh.transformed_mesh());
collector.collect(minkowski.predict_collision_out(&relative_body,start_time..collector.time()).map(|(_face,time)|{
let minkowski=MinkowskiMesh::minkowski_sum(model_mesh,hitbox_mesh.transformed_mesh());
collector.collect(minkowski.predict_collision_out(&trajectory,start_time..collector.time()).map(|(_face,time)|{
TimedInstruction{
time:relative_body.time+time.into(),
time:trajectory.time+time.into(),
instruction:InternalInstruction::CollisionEnd(
Collision::Intersect(IntersectCollision{convex_mesh_id:*convex_mesh_id}),
time
@@ -886,7 +880,7 @@ pub struct PhysicsState{
impl Default for PhysicsState{
fn default()->Self{
Self{
body:Body::new(vec3::int(0,50,0),vec3::int(0,0,0),vec3::int(0,-100,0),Time::ZERO),
body:Body::new(vec3::int(0,50,0),vec3::int(0,0,0),Time::ZERO),
time:Time::ZERO,
style:StyleModifiers::default(),
touching:TouchingState::default(),
@@ -910,11 +904,14 @@ impl PhysicsState{
pub const fn body(&self)->&Body{
&self.body
}
pub fn camera_body(&self)->Body{
Body{
position:self.body.position+self.style.camera_offset,
..self.body
}
pub fn camera_trajectory(&self,data:&PhysicsData)->Trajectory{
let acceleration=self.acceleration(data);
Trajectory::new(
self.body.position+self.style.camera_offset,
self.body.velocity,
acceleration,
self.body.time,
)
}
pub const fn camera(&self)->PhysicsCamera{
self.camera
@@ -928,6 +925,9 @@ impl PhysicsState{
pub fn get_finish_time(&self)->Option<run::Time>{
self.run.get_finish_time()
}
fn is_no_clip_enabled(&self)->bool{
self.input_state.get_control(Controls::Sprint)
}
pub fn clear(&mut self){
self.touching.clear();
}
@@ -943,8 +943,8 @@ impl PhysicsState{
fn set_move_state(&mut self,data:&PhysicsData,move_state:MoveState){
self.move_state.set_move_state(move_state,&mut self.body,&self.touching,&data.models,&data.hitbox_mesh,&self.style,&self.camera,&self.input_state);
}
fn apply_input_and_body(&mut self,data:&PhysicsData){
self.move_state.apply_input_and_body(&mut self.body,&self.touching,&data.models,&data.hitbox_mesh,&self.style,&self.camera,&self.input_state);
fn acceleration(&self,data:&PhysicsData)->Planar64Vec3{
self.move_state.acceleration(&self.touching,&data.models,&data.hitbox_mesh,&self.style,&self.camera,&self.input_state)
}
//state mutated on collision:
//Accelerator
@@ -1191,53 +1191,58 @@ impl<'a> PhysicsContext<'a>{
}
}
//this is the one who asks
fn next_instruction_internal(state:&PhysicsState,data:&PhysicsData,time_limit:Time)->Option<TimedInstruction<InternalInstruction,Time>>{
//JUST POLLING!!! NO MUTATION
let mut collector=instruction::InstructionCollector::new(time_limit);
//this is the one who asks
fn next_instruction_internal(state:&PhysicsState,data:&PhysicsData,time_limit:Time)->Option<TimedInstruction<InternalInstruction,Time>>{
//JUST POLLING!!! NO MUTATION
let mut collector=instruction::InstructionCollector::new(time_limit);
collector.collect(state.next_move_instruction());
collector.collect(state.next_move_instruction());
//check for collision ends
state.touching.predict_collision_end(&mut collector,&data.models,&data.hitbox_mesh,&state.body,state.time);
//check for collision starts
let mut aabb=aabb::Aabb::default();
state.body.grow_aabb(&mut aabb,state.time,collector.time());
aabb.inflate(data.hitbox_mesh.halfsize);
//relative to moving platforms
//let relative_body=state.body.relative_to(&Body::ZERO);
let relative_body=&state.body;
data.bvh.sample_aabb(&aabb,&mut |convex_mesh_id|{
if state.touching.contains(convex_mesh_id){
return;
}
//no checks are needed because of the time limits.
let model_mesh=data.models.mesh(*convex_mesh_id);
let minkowski=model_physics::MinkowskiMesh::minkowski_sum(model_mesh,data.hitbox_mesh.transformed_mesh());
collector.collect(minkowski.predict_collision_in(relative_body,state.time..collector.time())
.map(|(face,dt)|
TimedInstruction{
time:relative_body.time+dt.into(),
instruction:InternalInstruction::CollisionStart(
Collision::new(*convex_mesh_id,face),
dt
)
}
)
);
});
collector.take()
let trajectory=state.body.with_acceleration(state.acceleration(data));
//check for collision ends
state.touching.predict_collision_end(&mut collector,&data.models,&data.hitbox_mesh,&trajectory,state.time);
if state.is_no_clip_enabled(){
return collector.take();
}
//check for collision starts
let mut aabb=aabb::Aabb::default();
trajectory.grow_aabb(&mut aabb,state.time,collector.time());
aabb.inflate(data.hitbox_mesh.halfsize);
//relative to moving platforms
//let relative_body=state.body.relative_to(&Body::ZERO);
data.bvh.sample_aabb(&aabb,&mut |convex_mesh_id|{
if state.touching.contains(convex_mesh_id){
return;
}
//no checks are needed because of the time limits.
let model_mesh=data.models.mesh(*convex_mesh_id);
let minkowski=MinkowskiMesh::minkowski_sum(model_mesh,data.hitbox_mesh.transformed_mesh());
collector.collect(minkowski.predict_collision_in(&trajectory,state.time..collector.time())
.map(|(face,dt)|
TimedInstruction{
time:trajectory.time+dt.into(),
instruction:InternalInstruction::CollisionStart(
Collision::new(*convex_mesh_id,face),
dt
)
}
)
);
});
collector.take()
}
fn contact_normal(
models:&PhysicsModels,
hitbox_mesh:&HitboxMesh,
convex_mesh_id:&ConvexMeshId<ContactModelId>,
face_id:model_physics::MinkowskiFace,
face_id:MinkowskiFace,
)->Planar64Vec3{
let model_mesh=models.contact_mesh(convex_mesh_id);
let minkowski=model_physics::MinkowskiMesh::minkowski_sum(model_mesh,hitbox_mesh.transformed_mesh());
let minkowski=MinkowskiMesh::minkowski_sum(model_mesh,hitbox_mesh.transformed_mesh());
// TODO: normalize to i64::MAX>>1
// wrap for speed
minkowski.face_nd(face_id).0.wrap_1()
@@ -1276,8 +1281,8 @@ fn recalculate_touching(
bvh.sample_aabb(&aabb,&mut |&convex_mesh_id|{
//no checks are needed because of the time limits.
let model_mesh=models.mesh(convex_mesh_id);
let minkowski=model_physics::MinkowskiMesh::minkowski_sum(model_mesh,hitbox_mesh.transformed_mesh());
if minkowski.is_point_in_mesh(body.position){
let minkowski=MinkowskiMesh::minkowski_sum(model_mesh,hitbox_mesh.transformed_mesh());
if minkowski.contains_point(body.position){
match convex_mesh_id.model_id{
//being inside of contact objects is an invalid physics state
//but the physics isn't advanced enough to do anything about it yet
@@ -1335,24 +1340,6 @@ fn set_velocity_cull(body:&mut Body,touching:&mut TouchingState,models:&PhysicsM
fn set_velocity(body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,v:Planar64Vec3){
body.velocity=touching.constrain_velocity(models,hitbox_mesh,v);
}
#[expect(dead_code)]
fn set_acceleration_cull(body:&mut Body,touching:&mut TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,a:Planar64Vec3)->bool{
//This is not correct but is better than what I have
let mut culled=false;
touching.contacts.retain(|convex_mesh_id,face_id|{
let n=contact_normal(models,hitbox_mesh,convex_mesh_id,*face_id);
let r=n.dot(a).is_positive();
if r{
culled=true;
}
!r
});
set_acceleration(body,touching,models,hitbox_mesh,a);
culled
}
fn set_acceleration(body:&mut Body,touching:&TouchingState,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,a:Planar64Vec3){
body.acceleration=touching.constrain_acceleration(models,hitbox_mesh,a);
}
fn teleport(
point:Planar64Vec3,
@@ -1371,7 +1358,6 @@ fn teleport(
time:Time,
){
set_position(point,move_state,body,touching,run,mode_state,mode,models,hitbox_mesh,bvh,style,camera,input_state,time);
set_acceleration(body,touching,models,hitbox_mesh,style.gravity);
}
enum TeleportToSpawnError{
NoModel,
@@ -1634,7 +1620,8 @@ fn collision_start_contact(
}
//doing enum to set the acceleration when surfing
//doing input_and_body to refresh the walk state if you hit a wall while accelerating
move_state.apply_enum_and_input_and_body(body,touching,models,hitbox_mesh,style,camera,input_state);
move_state.update_walk_target(body,touching,models,hitbox_mesh,style,camera,input_state);
move_state.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state);
}
fn collision_start_intersect(
@@ -1669,7 +1656,20 @@ fn collision_start_intersect(
},
Some(gameplay_modes::Zone::Finish)=>{
match run.finish(time){
Ok(())=>println!("@@@@ Finished run time={}",run.time(time)),
Ok(())=>{
let time=run.time(time);
let h=time.get()/(Time::ONE_SECOND.get()*60*60);
let m=(time.get()/(Time::ONE_SECOND.get()*60)).rem_euclid(60);
let s=(time.get()/(Time::ONE_SECOND.get())).rem_euclid(60);
let ms=(time.get()/(Time::ONE_MILLISECOND.get())).rem_euclid(1000);
let us=(time.get()/(Time::ONE_MICROSECOND.get())).rem_euclid(1000);
let ns=(time.get()/(Time::ONE_NANOSECOND.get())).rem_euclid(1000);
if h==0{
println!("@@@@ Finished run time={m:02}:{s:02}.{ms:03}_{us:03}_{ns:03}");
}else{
println!("@@@@ Finished run time={h}:{m:02}:{s:02}.{ms:03}_{us:03}_{ns:03}");
}
},
Err(e)=>println!("@@@@ Run Finish error:{e:?}"),
}
},
@@ -1677,7 +1677,7 @@ fn collision_start_intersect(
None=>(),
}
}
move_state.apply_enum_and_body(body,touching,models,hitbox_mesh,style,camera,input_state);
move_state.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state);
run_teleport_behaviour(intersect.convex_mesh_id.model_id.into(),attr.general.wormhole.as_ref(),mode,move_state,body,touching,run,mode_state,models,hitbox_mesh,bvh,style,camera,input_state,time);
}
@@ -1705,10 +1705,11 @@ fn collision_end_contact(
move_state.set_move_state(MoveState::Air,body,touching,models,hitbox_mesh,style,camera,input_state);
}else{
// stopped touching something else while walking
move_state.apply_enum_and_input_and_body(body,touching,models,hitbox_mesh,style,camera,input_state);
move_state.update_walk_target(body,touching,models,hitbox_mesh,style,camera,input_state);
move_state.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state);
},
// not walking, but stopped touching something
None=>move_state.apply_enum_and_body(body,touching,models,hitbox_mesh,style,camera,input_state),
None=>move_state.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state),
}
}
fn collision_end_intersect(
@@ -1727,7 +1728,7 @@ fn collision_end_intersect(
time:Time,
){
touching.remove_intersect(convex_mesh_id);
move_state.apply_enum_and_body(body,touching,models,hitbox_mesh,style,camera,input_state);
move_state.update_fly_velocity(body,touching,models,hitbox_mesh,style,camera,input_state);
if let Some(mode)=mode{
let zone=mode.get_zone(convex_mesh_id.model_id.into());
match zone{
@@ -1743,99 +1744,100 @@ fn collision_end_intersect(
}
fn atomic_internal_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedInstruction<InternalInstruction,Time>){
state.time=ins.time;
let (should_advance_body,goober_time)=match ins.instruction{
match ins.instruction{
// collisions advance the body precisely
InternalInstruction::CollisionStart(_,dt)
|InternalInstruction::CollisionEnd(_,dt)=>(true,Some(dt)),
InternalInstruction::StrafeTick
|InternalInstruction::ReachWalkTargetVelocity=>(true,None),
};
if should_advance_body{
match goober_time{
Some(dt)=>state.body.advance_time_ratio_dt(dt),
None=>state.body.advance_time(state.time),
}
|InternalInstruction::CollisionEnd(_,dt)=>{
state.body=state.body.with_acceleration(state.acceleration(data)).extrapolated_body_ratio_dt(dt);
},
// this advances imprecisely
InternalInstruction::ReachWalkTargetVelocity=>state.body=state.body.with_acceleration(state.acceleration(data)).extrapolated_body(state.time),
// strafe tick decides for itself whether to advance the body.
InternalInstruction::StrafeTick=>(),
}
match ins.instruction{
InternalInstruction::CollisionStart(collision,_)=>{
let mode=data.modes.get_mode(state.mode_state.get_mode_id());
match collision{
Collision::Contact(contact)=>collision_start_contact(
&mut state.move_state,&mut state.body,&mut state.mode_state,&mut state.touching,&mut state.run,
mode,
&data.models,&data.hitbox_mesh,&data.bvh,&state.style,&state.camera,&state.input_state,
data.models.contact_attr(contact.convex_mesh_id.model_id),
contact,
state.time,
),
Collision::Intersect(intersect)=>collision_start_intersect(
&mut state.move_state,&mut state.body,&mut state.mode_state,&mut state.touching,
mode,
&mut state.run,&data.models,&data.hitbox_mesh,&data.bvh,&state.style,&state.camera,&state.input_state,
data.models.intersect_attr(intersect.convex_mesh_id.model_id),
intersect,
state.time,
),
}
},
InternalInstruction::CollisionEnd(collision,_)=>match collision{
Collision::Contact(contact)=>collision_end_contact(
&mut state.move_state,&mut state.body,&mut state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state,
match ins.instruction{
InternalInstruction::CollisionStart(collision,_)=>{
let mode=data.modes.get_mode(state.mode_state.get_mode_id());
match collision{
Collision::Contact(contact)=>collision_start_contact(
&mut state.move_state,&mut state.body,&mut state.mode_state,&mut state.touching,&mut state.run,
mode,
&data.models,&data.hitbox_mesh,&data.bvh,&state.style,&state.camera,&state.input_state,
data.models.contact_attr(contact.convex_mesh_id.model_id),
&contact.convex_mesh_id
contact,
state.time,
),
Collision::Intersect(intersect)=>collision_end_intersect(
&mut state.move_state,&mut state.body,&mut state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state,
data.modes.get_mode(state.mode_state.get_mode_id()),
&mut state.run,
Collision::Intersect(intersect)=>collision_start_intersect(
&mut state.move_state,&mut state.body,&mut state.mode_state,&mut state.touching,
mode,
&mut state.run,&data.models,&data.hitbox_mesh,&data.bvh,&state.style,&state.camera,&state.input_state,
data.models.intersect_attr(intersect.convex_mesh_id.model_id),
&intersect.convex_mesh_id,
state.time
intersect,
state.time,
),
},
InternalInstruction::StrafeTick=>{
//TODO make this less huge
if let Some(strafe_settings)=&state.style.strafe{
let controls=state.input_state.controls;
if strafe_settings.activates(controls){
let masked_controls=strafe_settings.mask(controls);
let control_dir=state.style.get_control_dir(masked_controls);
if control_dir!=vec3::zero(){
let camera_mat=state.camera.simulate_move_rotation_y(state.input_state.lerp_delta(state.time).x);
if let Some(ticked_velocity)=strafe_settings.tick_velocity(state.body.velocity,(camera_mat*control_dir).with_length(Planar64::ONE).divide().wrap_1()){
//this is wrong but will work ig
//need to note which push planes activate in push solve and keep those
state.cull_velocity(data,ticked_velocity);
}
}
},
InternalInstruction::CollisionEnd(collision,_)=>match collision{
Collision::Contact(contact)=>collision_end_contact(
&mut state.move_state,&mut state.body,&mut state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state,
data.models.contact_attr(contact.convex_mesh_id.model_id),
&contact.convex_mesh_id
),
Collision::Intersect(intersect)=>collision_end_intersect(
&mut state.move_state,&mut state.body,&mut state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state,
data.modes.get_mode(state.mode_state.get_mode_id()),
&mut state.run,
data.models.intersect_attr(intersect.convex_mesh_id.model_id),
&intersect.convex_mesh_id,
state.time
),
},
InternalInstruction::StrafeTick=>{
//TODO make this less huge
if let Some(strafe_settings)=&state.style.strafe{
let controls=state.input_state.controls;
if strafe_settings.activates(controls){
let masked_controls=strafe_settings.mask(controls);
let control_dir=state.style.get_control_dir(masked_controls);
if control_dir!=vec3::zero(){
// manually advance time
let extrapolated_body=state.body.with_acceleration(state.acceleration(data)).extrapolated_body(state.time);
let camera_mat=state.camera.simulate_move_rotation_y(state.input_state.lerp_delta(state.time).x);
if let Some(ticked_velocity)=strafe_settings.tick_velocity(extrapolated_body.velocity,(camera_mat*control_dir).with_length(Planar64::ONE).divide().wrap_1()){
state.body=extrapolated_body;
//this is wrong but will work ig
//need to note which push planes activate in push solve and keep those
state.cull_velocity(data,ticked_velocity);
}
}
}
}
InternalInstruction::ReachWalkTargetVelocity=>{
match &mut state.move_state{
MoveState::Air
|MoveState::Water
|MoveState::Fly
=>println!("ReachWalkTargetVelocity fired for non-walking MoveState"),
MoveState::Walk(walk_state)|MoveState::Ladder(walk_state)=>{
//velocity is already handled by advance_time
//we know that the acceleration is precisely zero because the walk target is known to be reachable
//which means that gravity can be fully cancelled
//ignore moving platforms for now
let target=core::mem::replace(&mut walk_state.target,TransientAcceleration::Reached);
set_acceleration(&mut state.body,&state.touching,&data.models,&data.hitbox_mesh,vec3::zero());
// check what the target was to see if it was invalid
match target{
//you are not supposed to reach a walk target which is already reached!
TransientAcceleration::Reached=>println!("Invalid walk target: Reached"),
TransientAcceleration::Reachable{..}=>(),
//you are not supposed to reach an unreachable walk target!
TransientAcceleration::Unreachable{..}=>println!("Invalid walk target: Unreachable"),
}
}
InternalInstruction::ReachWalkTargetVelocity=>{
match &mut state.move_state{
MoveState::Air
|MoveState::Water
|MoveState::Fly
=>println!("ReachWalkTargetVelocity fired for non-walking MoveState"),
MoveState::Walk(walk_state)|MoveState::Ladder(walk_state)=>{
//velocity is already handled by extrapolated_body
//we know that the acceleration is precisely zero because the walk target is known to be reachable
//which means that gravity can be fully cancelled
//ignore moving platforms for now
let target=core::mem::replace(&mut walk_state.target,TransientAcceleration::Reached);
// check what the target was to see if it was invalid
match target{
//you are not supposed to reach a walk target which is already reached!
TransientAcceleration::Reached=>println!("Invalid walk target: Reached"),
TransientAcceleration::Reachable{..}=>(),
//you are not supposed to reach an unreachable walk target!
TransientAcceleration::Unreachable{..}=>println!("Invalid walk target: Unreachable"),
}
}
},
}
}
},
}
}
fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedInstruction<Instruction,Time>){
state.time=ins.time;
@@ -1861,7 +1863,7 @@ fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedI
Instruction::Misc(MiscInstruction::PracticeFly)=>true,
};
if should_advance_body{
state.body.advance_time(state.time);
state.body=state.body.with_acceleration(state.acceleration(data)).extrapolated_body(state.time);
}
let mut b_refresh_walk_target=true;
@@ -1897,6 +1899,9 @@ fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedI
state.input_state.set_control(Controls::Zoom,s);
b_refresh_walk_target=false;
},
Instruction::SetControl(SetControlInstruction::SetSprint(s))=>{
state.input_state.set_control(Controls::Sprint,s);
},
Instruction::Mode(ModeInstruction::Reset)=>{
//totally reset physics state
state.reset_to_default();
@@ -1956,7 +1961,8 @@ fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedI
},
}
if b_refresh_walk_target{
state.apply_input_and_body(data);
state.move_state.update_walk_target(&mut state.body,&state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state);
state.move_state.update_fly_velocity(&mut state.body,&state.touching,&data.models,&data.hitbox_mesh,&state.style,&state.camera,&state.input_state);
state.cull_velocity(data,state.body.velocity);
//also check if accelerating away from surface
}
@@ -1966,16 +1972,16 @@ fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedI
mod test{
use strafesnet_common::integer::{vec3::{self,int as int3},mat3};
use super::*;
fn test_collision_axis_aligned(relative_body:Body,expected_collision_time:Option<Time>){
fn test_collision_axis_aligned(relative_body:Trajectory,expected_collision_time:Option<Time>){
let h0=HitboxMesh::new(PhysicsMesh::unit_cube(),integer::Planar64Affine3::new(mat3::from_diagonal(int3(5,1,5)>>1),vec3::zero()));
let h1=StyleModifiers::roblox_bhop().calculate_mesh();
let hitbox_mesh=h1.transformed_mesh();
let platform_mesh=h0.transformed_mesh();
let minkowski=model_physics::MinkowskiMesh::minkowski_sum(platform_mesh,hitbox_mesh);
let collision=minkowski.predict_collision_in(&relative_body,Time::ZERO..Time::from_secs(10));
let minkowski=MinkowskiMesh::minkowski_sum(platform_mesh,hitbox_mesh);
let collision=minkowski.predict_collision_in(&relative_body,..);
assert_eq!(collision.map(|tup|relative_body.time+tup.1.into()),expected_collision_time,"Incorrect time of collision");
}
fn test_collision_rotated(relative_body:Body,expected_collision_time:Option<Time>){
fn test_collision_rotated(relative_body:Trajectory,expected_collision_time:Option<Time>){
let h0=HitboxMesh::new(PhysicsMesh::unit_cube(),
integer::Planar64Affine3::new(
Planar64Mat3::from_cols([
@@ -1989,17 +1995,17 @@ mod test{
let h1=StyleModifiers::roblox_bhop().calculate_mesh();
let hitbox_mesh=h1.transformed_mesh();
let platform_mesh=h0.transformed_mesh();
let minkowski=model_physics::MinkowskiMesh::minkowski_sum(platform_mesh,hitbox_mesh);
let collision=minkowski.predict_collision_in(&relative_body,Time::ZERO..Time::from_secs(10));
let minkowski=MinkowskiMesh::minkowski_sum(platform_mesh,hitbox_mesh);
let collision=minkowski.predict_collision_in(&relative_body,..);
assert_eq!(collision.map(|tup|relative_body.time+tup.1.into()),expected_collision_time,"Incorrect time of collision");
}
fn test_collision(relative_body:Body,expected_collision_time:Option<Time>){
fn test_collision(relative_body:Trajectory,expected_collision_time:Option<Time>){
test_collision_axis_aligned(relative_body,expected_collision_time);
test_collision_rotated(relative_body,expected_collision_time);
}
#[test]
fn test_collision_degenerate_straight_down(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,5,0),
int3(0,-1,0),
vec3::zero(),
@@ -2008,7 +2014,7 @@ mod test{
}
#[test]
fn test_collision_small_mv(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,5,0),
int3(0,-1,0)+(vec3::X>>32),
vec3::zero(),
@@ -2017,7 +2023,7 @@ mod test{
}
#[test]
fn test_collision_degenerate_east(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(3,5,0),
int3(0,-1,0),
vec3::zero(),
@@ -2026,7 +2032,7 @@ mod test{
}
#[test]
fn test_collision_degenerate_south(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,5,3),
int3(0,-1,0),
vec3::zero(),
@@ -2035,7 +2041,7 @@ mod test{
}
#[test]
fn test_collision_degenerate_west(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(-3,5,0),
int3(0,-1,0),
vec3::zero(),
@@ -2044,7 +2050,7 @@ mod test{
}
#[test]
fn test_collision_degenerate_north(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,5,-3),
int3(0,-1,0),
vec3::zero(),
@@ -2053,115 +2059,115 @@ mod test{
}
#[test]
fn test_collision_parabola_edge_east_from_west(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(3,3,0),
int3(100,-1,0),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_south_from_north(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,3,3),
int3(0,-1,100),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_west_from_east(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(-3,3,0),
int3(-100,-1,0),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_north_from_south(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,3,-3),
int3(0,-1,-100),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_north_from_ne(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,6,-7)>>1,
int3(-10,-1,1),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_north_from_nw(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,6,-7)>>1,
int3(10,-1,1),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_east_from_se(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(7,6,0)>>1,
int3(-1,-1,-10),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_east_from_ne(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(7,6,0)>>1,
int3(-1,-1,10),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_south_from_se(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,6,7)>>1,
int3(-10,-1,-1),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_south_from_sw(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,6,7)>>1,
int3(10,-1,-1),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_west_from_se(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(-7,6,0)>>1,
int3(1,-1,-10),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_parabola_edge_west_from_ne(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(-7,6,0)>>1,
int3(1,-1,10),
int3(0,-1,0),
Time::ZERO
).relative_to(&Body::ZERO).body(Time::from_secs(-1)),Some(Time::from_secs(0)));
).relative_to(&Trajectory::ZERO,Time::from_secs(-1)),Some(Time::from_secs(0)));
}
#[test]
fn test_collision_oblique(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,5,0),
int3(1,-64,2)>>6,// /64
vec3::zero(),
@@ -2170,7 +2176,7 @@ mod test{
}
#[test]
fn zoom_hit_nothing(){
test_collision(Body::new(
test_collision(Trajectory::new(
int3(0,10,0),
int3(1,0,0),
int3(0,1,0),
@@ -2179,7 +2185,7 @@ mod test{
}
#[test]
fn already_inside_hit_nothing(){
test_collision(Body::new(
test_collision(Trajectory::new(
vec3::zero(),
int3(1,0,0),
int3(0,1,0),
@@ -2189,7 +2195,7 @@ mod test{
// overlap edges by 1 epsilon
#[test]
fn almost_miss_north(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
(int3(0,10,-7)>>1)+vec3::raw_xyz(0,0,1),
int3(0,-1,0),
vec3::zero(),
@@ -2198,7 +2204,7 @@ mod test{
}
#[test]
fn almost_miss_east(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
(int3(7,10,0)>>1)+vec3::raw_xyz(-1,0,0),
int3(0,-1,0),
vec3::zero(),
@@ -2207,7 +2213,7 @@ mod test{
}
#[test]
fn almost_miss_south(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
(int3(0,10,7)>>1)+vec3::raw_xyz(0,0,-1),
int3(0,-1,0),
vec3::zero(),
@@ -2216,7 +2222,7 @@ mod test{
}
#[test]
fn almost_miss_west(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
(int3(-7,10,0)>>1)+vec3::raw_xyz(1,0,0),
int3(0,-1,0),
vec3::zero(),
@@ -2226,7 +2232,7 @@ mod test{
// exactly miss edges
#[test]
fn exact_miss_north(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
int3(0,10,-7)>>1,
int3(0,-1,0),
vec3::zero(),
@@ -2235,7 +2241,7 @@ mod test{
}
#[test]
fn exact_miss_east(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
int3(7,10,0)>>1,
int3(0,-1,0),
vec3::zero(),
@@ -2244,7 +2250,7 @@ mod test{
}
#[test]
fn exact_miss_south(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
int3(0,10,7)>>1,
int3(0,-1,0),
vec3::zero(),
@@ -2253,7 +2259,7 @@ mod test{
}
#[test]
fn exact_miss_west(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
int3(-7,10,0)>>1,
int3(0,-1,0),
vec3::zero(),
@@ -2263,7 +2269,7 @@ mod test{
// miss edges by 1 epsilon
#[test]
fn narrow_miss_north(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
(int3(0,10,-7)>>1)-vec3::raw_xyz(0,0,1),
int3(0,-1,0),
vec3::zero(),
@@ -2272,7 +2278,7 @@ mod test{
}
#[test]
fn narrow_miss_east(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
(int3(7,10,0)>>1)-vec3::raw_xyz(-1,0,0),
int3(0,-1,0),
vec3::zero(),
@@ -2281,7 +2287,7 @@ mod test{
}
#[test]
fn narrow_miss_south(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
(int3(0,10,7)>>1)-vec3::raw_xyz(0,0,-1),
int3(0,-1,0),
vec3::zero(),
@@ -2290,7 +2296,7 @@ mod test{
}
#[test]
fn narrow_miss_west(){
test_collision_axis_aligned(Body::new(
test_collision_axis_aligned(Trajectory::new(
(int3(-7,10,0)>>1)-vec3::raw_xyz(1,0,0),
int3(0,-1,0),
vec3::zero(),

View File

@@ -15,6 +15,7 @@ type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>;
const RATIO_ZERO:Ratio<Fixed<1,32>,Fixed<1,32>>=Ratio::new(Fixed::ZERO,Fixed::EPSILON);
/// Information about a contact restriction
#[derive(Debug,PartialEq)]
pub struct Contact{
pub position:Planar64Vec3,
pub velocity:Planar64Vec3,
@@ -281,16 +282,16 @@ fn get_first_touch<'a>(contacts:&'a [Contact],ray:&Ray,conts:&Conts)->Option<(Ra
.min_by_key(|&(t,_)|t)
}
pub fn push_solve(contacts:&[Contact],point:Planar64Vec3)->Planar64Vec3{
pub fn push_solve(contacts:&[Contact],point:Planar64Vec3)->(Planar64Vec3,Conts<'_>){
let (mut ray,mut conts)=get_best_push_ray_and_conts_0(point);
loop{
let (next_t,next_cont)=match get_first_touch(contacts,&ray,&conts){
Some((t,cont))=>(t,cont),
None=>return ray.origin,
None=>return (ray.origin,conts),
};
if RATIO_ZERO.le_ratio(next_t){
return ray.origin;
return (ray.origin,conts);
}
//push_front
@@ -306,7 +307,7 @@ pub fn push_solve(contacts:&[Contact],point:Planar64Vec3)->Planar64Vec3{
let meet_point=ray.extrapolate(next_t);
match get_best_push_ray_and_conts(meet_point,conts.as_slice()){
Some((new_ray,new_conts))=>(ray,conts)=(new_ray,new_conts),
None=>return meet_point,
None=>return (meet_point,conts),
}
}
}
@@ -323,9 +324,8 @@ mod tests{
normal:vec3::Y,
}
];
assert_eq!(
vec3::zero(),
push_solve(&contacts,vec3::NEG_Y)
);
let (point,conts)=push_solve(&contacts,vec3::NEG_Y);
assert_eq!(point,vec3::zero());
assert_eq!(conts.as_slice(),[&contacts[0]].as_slice());
}
}

View File

@@ -57,7 +57,7 @@ pub enum SessionPlaybackInstruction{
}
pub struct FrameState{
pub body:physics::Body,
pub trajectory:physics::Trajectory,
pub camera:physics::PhysicsCamera,
pub time:PhysicsTime,
}
@@ -76,9 +76,9 @@ impl Simulation{
physics,
}
}
pub fn get_frame_state(&self,time:SessionTime)->FrameState{
pub fn get_frame_state(&self,time:SessionTime,data:&PhysicsData)->FrameState{
FrameState{
body:self.physics.camera_body(),
trajectory:self.physics.camera_trajectory(data),
camera:self.physics.camera(),
time:self.timer.time(time),
}
@@ -187,9 +187,9 @@ impl Session{
}
pub fn get_frame_state(&self,time:SessionTime)->Option<FrameState>{
match &self.view_state{
ViewState::Play=>Some(self.simulation.get_frame_state(time)),
ViewState::Play=>Some(self.simulation.get_frame_state(time,&self.geometry_shared)),
ViewState::Replay(bot_id)=>self.replays.get(bot_id).map(|replay|
replay.simulation.get_frame_state(time)
replay.simulation.get_frame_state(time,&self.geometry_shared)
),
}
}
@@ -238,12 +238,16 @@ impl InstructionConsumer<Instruction<'_>> for Session{
run_mouse_interpolator_instruction!(MouseInterpolatorInstruction::Mode(ModeInstruction::Reset));
run_mouse_interpolator_instruction!(MouseInterpolatorInstruction::Misc(MiscInstruction::SetSensitivity(self.user_settings().calculate_sensitivity())));
run_mouse_interpolator_instruction!(MouseInterpolatorInstruction::Mode(ModeInstruction::Restart(mode_id)));
// TODO: think about this harder. This works around a bug where you fall infinitely when you reset.
self.simulation.timer.set_time(ins.time,PhysicsTime::ZERO);
},
Instruction::Input(SessionInputInstruction::Mode(ImplicitModeInstruction::ResetAndSpawn(mode_id,spawn_id)))=>{
self.clear_recording();
run_mouse_interpolator_instruction!(MouseInterpolatorInstruction::Mode(ModeInstruction::Reset));
run_mouse_interpolator_instruction!(MouseInterpolatorInstruction::Misc(MiscInstruction::SetSensitivity(self.user_settings().calculate_sensitivity())));
run_mouse_interpolator_instruction!(MouseInterpolatorInstruction::Mode(ModeInstruction::Spawn(mode_id,spawn_id)));
// TODO: think about this harder. This works around a bug where you fall infinitely when you reset.
self.simulation.timer.set_time(ins.time,PhysicsTime::ZERO);
},
Instruction::Input(SessionInputInstruction::Misc(misc_instruction))=>{
run_mouse_interpolator_instruction!(MouseInterpolatorInstruction::Misc(misc_instruction));

View File

@@ -71,16 +71,16 @@ fn segment_determinism(bot:strafesnet_snf::bot::Segment,physics_data:&PhysicsDat
for (i,ins) in bot.instructions.into_iter().enumerate(){
let state_deterministic=physics_deterministic.clone();
let state_filtered=physics_filtered.clone();
PhysicsContext::run_input_instruction(&mut physics_deterministic,&physics_data,ins.clone());
PhysicsContext::run_input_instruction(&mut physics_deterministic,physics_data,ins.clone());
match ins{
strafesnet_common::instruction::TimedInstruction{instruction:strafesnet_common::physics::Instruction::Idle,..}=>(),
other=>{
non_idle_count+=1;
// run
PhysicsContext::run_input_instruction(&mut physics_filtered,&physics_data,other.clone());
PhysicsContext::run_input_instruction(&mut physics_filtered,physics_data,other.clone());
// check if position matches
let b0=physics_deterministic.camera_body();
let b1=physics_filtered.camera_body();
let b0=physics_deterministic.camera_trajectory(physics_data);
let b1=physics_filtered.camera_trajectory(physics_data);
if b0.position!=b1.position{
let nanoseconds=start.elapsed().as_nanos() as u64;
println!("desync at instruction #{}",i);

View File

@@ -73,7 +73,6 @@ fn simultaneous_collision(){
let body=strafesnet_physics::physics::Body::new(
(vec3::int(5+2,0,0)>>1)+vec3::int(1,1,0),
vec3::int(-1,-1,0),
vec3::int(0,0,0),
Time::ZERO,
);
let mut physics=PhysicsState::new_with_body(body);
@@ -88,7 +87,6 @@ fn simultaneous_collision(){
let body=physics.body();
assert_eq!(body.position,vec3::int(5,0,0));
assert_eq!(body.velocity,vec3::int(0,0,0));
assert_eq!(body.acceleration,vec3::int(0,0,0));
assert_eq!(body.time,Time::from_secs(1));
}
#[test]
@@ -97,7 +95,6 @@ fn bug_3(){
let body=strafesnet_physics::physics::Body::new(
(vec3::int(5+2,0,0)>>1)+vec3::int(1,2,0),
vec3::int(-1,-1,0),
vec3::int(0,0,0),
Time::ZERO,
);
let mut physics=PhysicsState::new_with_body(body);
@@ -112,6 +109,5 @@ fn bug_3(){
let body=physics.body();
assert_eq!(body.position,vec3::int(5+2,0,0)>>1);
assert_eq!(body.velocity,vec3::int(0,0,0));
assert_eq!(body.acceleration,vec3::int(0,0,0));
assert_eq!(body.time,Time::from_secs(2));
}

View File

@@ -22,7 +22,6 @@ fn physics_bug_2()->Result<(),ReplayError>{
let body=strafesnet_physics::physics::Body::new(
vec3::raw_xyz(555690659654,1490485868773,1277783839382),
vec3::int(0,0,0),
vec3::int(0,-100,0),
Time::ZERO,
);
let mut physics=PhysicsState::new_with_body(body);
@@ -60,9 +59,12 @@ fn physics_bug_3()->Result<(),ReplayError>{
// vec3::raw_xyz(0,-96915585363,1265),
// vec3::raw_xyz(0,-429496729600,0),
// corner setup before wall hits
vec3::raw_xyz(-1392580080675,3325402529458,-2444727738679),
vec3::raw_xyz(-30259028820,-22950929553,-71141663007),
vec3::raw_xyz(0,-429496729600,0),
// vec3::raw_xyz(-1392580080675,3325402529458,-2444727738679),
// vec3::raw_xyz(-30259028820,-22950929553,-71141663007),
// vec3::raw_xyz(0,-429496729600,0),
// Actual bug 3 repro
vec3::raw_xyz(-2505538624455,3357963283914,557275711118),
vec3::raw_xyz(204188283920,-282280474198,166172785440),
Time::ZERO,
);
let mut physics=PhysicsState::new_with_body(body);

View File

@@ -36,6 +36,7 @@ pub enum SetControlInstruction{
SetMoveForward(bool),
SetJump(bool),
SetZoom(bool),
SetSprint(bool),
}
#[derive(Clone,Debug)]
pub enum ModeInstruction{

View File

@@ -7,7 +7,7 @@ const BNUM_DIGIT_WIDTH:usize=64;
/// N is the number of u64s to use
/// F is the number of fractional bits (always N*32 lol)
pub struct Fixed<const N:usize,const F:usize>{
pub(crate)bits:BInt<{N}>,
bits:BInt<{N}>,
}
impl<const N:usize,const F:usize> Fixed<N,F>{
@@ -505,16 +505,10 @@ impl_multiplicative_assign_operator!( Fixed, DivAssign, div_assign, div_euclid )
impl_multiplicative_operator!( Fixed, Div, div, div_euclid, Self );
#[cfg(feature="deferred-division")]
impl<const LHS_N:usize,const LHS_F:usize,const RHS_N:usize,const RHS_F:usize> core::ops::Div<Fixed<RHS_N,RHS_F>> for Fixed<LHS_N,LHS_F>{
type Output=ratio_ops::ratio::Ratio<Fixed<LHS_N,LHS_F>,Fixed<RHS_N,RHS_F>>;
type Output=crate::ratio::Ratio<LHS_N,RHS_N,LHS_F,RHS_F>;
#[inline]
fn div(self, other: Fixed<RHS_N,RHS_F>)->Self::Output{
ratio_ops::ratio::Ratio::new(self,other)
}
}
#[cfg(feature="deferred-division")]
impl<const N:usize,const F:usize> ratio_ops::ratio::Parity for Fixed<N,F>{
fn parity(&self)->bool{
self.is_negative()
crate::ratio::Ratio::new(self,other)
}
}
macro_rules! impl_shift_operator {
@@ -545,7 +539,7 @@ impl_shift_operator!( Fixed, Shr, shr, Self );
// wide operators. The result width is the sum of the input widths, i.e. none of the multiplication
#[expect(unused_macros)]
#[allow(unused_macros)]
macro_rules! impl_wide_operators{
($lhs:expr,$rhs:expr)=>{
impl core::ops::Mul<Fixed<$rhs,{$rhs*32}>> for Fixed<$lhs,{$lhs*32}>{

View File

@@ -1,4 +1,5 @@
pub mod fixed;
pub mod ratio;
pub mod types;
#[cfg(feature="zeroes")]

View File

@@ -0,0 +1,22 @@
use bnum::{BInt,BUInt,cast::As};
use crate::fixed::Fixed;
pub struct Ratio<const N:usize,const D:usize,const NF:usize,const DF:usize>{
num:BInt<{N}>,
den:BUInt<{D}>,
}
// Fixed<N = 8 bits,NF = 4 frac> / Fixed<D = 8 bits,DF = 3 frac>
// 0100.0000/00100.000
// 01000000<<DF/00100000 = 10>>NF
impl Ratio{
/// Evaluate a ratio to a specific precision
pub fn evaluate<const OUT_N:usize,const OUT_F:usize>(&self)->Fixed<OUT_N,OUT_F>{
// TODO: Think (this is completely wrong)
// (lhs/2^LHS_FRAC)/(rhs/2^RHS_FRAC)
let lhs=num.bits.as_::<BInt<OUT_N>>().shl(OUT_N*64);
let rhs=rhs.bits.as_::<BInt<OUT_N>>();
Fixed::from_bits(lhs/rhs)
}
}

View File

@@ -1 +0,0 @@
/target

View File

@@ -1,13 +0,0 @@
[package]
name = "ratio_ops"
version = "0.1.1"
edition = "2024"
repository = "https://git.itzana.me/StrafesNET/strafe-project"
license = "MIT OR Apache-2.0"
description = "Ratio operations using trait bounds for avoiding division like the plague."
authors = ["Rhys Lloyd <krakow20@gmail.com>"]
[dependencies]
[lints]
workspace = true

View File

@@ -1,176 +0,0 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS

View File

@@ -1,23 +0,0 @@
Permission is hereby granted, free of charge, to any
person obtaining a copy of this software and associated
documentation files (the "Software"), to deal in the
Software without restriction, including without
limitation the rights to use, copy, modify, merge,
publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software
is furnished to do so, subject to the following
conditions:
The above copyright notice and this permission notice
shall be included in all copies or substantial portions
of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF
ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT
SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR
IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.

View File

@@ -1,4 +0,0 @@
pub mod ratio;
#[cfg(test)]
mod tests;

View File

@@ -1,302 +0,0 @@
#[derive(Clone,Copy,Debug,Hash)]
pub struct Ratio<Num,Den>{
pub num:Num,
pub den:Den,
}
impl<Num,Den> Ratio<Num,Den>{
#[inline(always)]
pub const fn new(num:Num,den:Den)->Self{
Self{num,den}
}
}
/// The actual divide implementation, Div is replaced with a Ratio constructor
pub trait Divide<Rhs=Self>{
type Output;
fn divide(self,rhs:Rhs)->Self::Output;
}
impl<Num,Den> Ratio<Num,Den>
where
Num:Divide<Den>,
{
#[inline]
pub fn divide(self)-><Num as Divide<Den>>::Output{
self.num.divide(self.den)
}
}
//take care to use the ratio methods to avoid nested ratios
impl<LhsNum,LhsDen> Ratio<LhsNum,LhsDen>{
#[inline]
pub fn mul_ratio<RhsNum,RhsDen>(self,rhs:Ratio<RhsNum,RhsDen>)->Ratio<<LhsNum as core::ops::Mul<RhsNum>>::Output,<LhsDen as core::ops::Mul<RhsDen>>::Output>
where
LhsNum:core::ops::Mul<RhsNum>,
LhsDen:core::ops::Mul<RhsDen>,
{
Ratio::new(self.num*rhs.num,self.den*rhs.den)
}
#[inline]
pub fn div_ratio<RhsNum,RhsDen>(self,rhs:Ratio<RhsNum,RhsDen>)->Ratio<<LhsNum as core::ops::Mul<RhsDen>>::Output,<LhsDen as core::ops::Mul<RhsNum>>::Output>
where
LhsNum:core::ops::Mul<RhsDen>,
LhsDen:core::ops::Mul<RhsNum>,
{
Ratio::new(self.num*rhs.den,self.den*rhs.num)
}
}
macro_rules! impl_ratio_method {
($trait:ident, $method:ident, $ratio_method:ident) => {
impl<LhsNum,LhsDen> Ratio<LhsNum,LhsDen>{
#[inline]
pub fn $ratio_method<RhsNum,RhsDen,LhsCrossMul,RhsCrossMul>(self,rhs:Ratio<RhsNum,RhsDen>)->Ratio<<LhsCrossMul as core::ops::$trait<RhsCrossMul>>::Output,<LhsDen as core::ops::Mul<RhsDen>>::Output>
where
LhsNum:core::ops::Mul<RhsDen,Output=LhsCrossMul>,
LhsDen:core::ops::Mul<RhsNum,Output=RhsCrossMul>,
LhsDen:core::ops::Mul<RhsDen>,
LhsDen:Copy,
RhsDen:Copy,
LhsCrossMul:core::ops::$trait<RhsCrossMul>,
{
Ratio::new((self.num*rhs.den).$method(self.den*rhs.num),self.den*rhs.den)
}
}
};
}
impl_ratio_method!(Add,add,add_ratio);
impl_ratio_method!(Sub,sub,sub_ratio);
impl_ratio_method!(Rem,rem,rem_ratio);
/// Comparing two ratios needs to know the parity of the denominators
/// For signed integers this can be implemented with is_negative()
pub trait Parity{
fn parity(&self)->bool;
}
macro_rules! impl_parity_unsigned{
($($type:ty),*)=>{
$(
impl Parity for $type{
fn parity(&self)->bool{
false
}
}
)*
};
}
macro_rules! impl_parity_signed{
($($type:ty),*)=>{
$(
impl Parity for $type{
fn parity(&self)->bool{
self.is_negative()
}
}
)*
};
}
macro_rules! impl_parity_float{
($($type:ty),*)=>{
$(
impl Parity for $type{
fn parity(&self)->bool{
self.is_sign_negative()
}
}
)*
};
}
impl_parity_unsigned!(u8,u16,u32,u64,u128,usize);
impl_parity_signed!(i8,i16,i32,i64,i128,isize);
impl_parity_float!(f32,f64);
macro_rules! impl_ratio_ord_method{
($method:ident, $ratio_method:ident, $output:ty)=>{
impl<LhsNum,LhsDen:Parity> Ratio<LhsNum,LhsDen>{
#[inline]
pub fn $ratio_method<RhsNum,RhsDen:Parity,T>(self,rhs:Ratio<RhsNum,RhsDen>)->$output
where
LhsNum:core::ops::Mul<RhsDen,Output=T>,
LhsDen:core::ops::Mul<RhsNum,Output=T>,
T:Ord,
{
match self.den.parity()^rhs.den.parity(){
true=>(self.den*rhs.num).$method(&(self.num*rhs.den)),
false=>(self.num*rhs.den).$method(&(self.den*rhs.num)),
}
}
}
}
}
//PartialEq
impl_ratio_ord_method!(eq,eq_ratio,bool);
//PartialOrd
impl_ratio_ord_method!(lt,lt_ratio,bool);
impl_ratio_ord_method!(gt,gt_ratio,bool);
impl_ratio_ord_method!(le,le_ratio,bool);
impl_ratio_ord_method!(ge,ge_ratio,bool);
impl_ratio_ord_method!(partial_cmp,partial_cmp_ratio,Option<core::cmp::Ordering>);
//Ord
impl_ratio_ord_method!(cmp,cmp_ratio,core::cmp::Ordering);
/* generic rhs mul is not possible!
impl<Lhs,RhsNum,RhsDen> core::ops::Mul<Ratio<RhsNum,RhsDen>> for Lhs
where
Lhs:core::ops::Mul<RhsNum>,
{
type Output=Ratio<<Lhs as core::ops::Mul<RhsNum>>::Output,RhsDen>;
#[inline]
fn mul(self,rhs:Ratio<RhsNum,RhsDen>)->Self::Output{
Ratio::new(self*rhs.num,rhs.den)
}
}
*/
//operators
impl<LhsNum,LhsDen> core::ops::Neg for Ratio<LhsNum,LhsDen>
where
LhsNum:core::ops::Neg,
{
type Output=Ratio<<LhsNum as core::ops::Neg>::Output,LhsDen>;
#[inline]
fn neg(self)->Self::Output{
Ratio::new(-self.num,self.den)
}
}
impl<LhsNum,LhsDen,Rhs> core::ops::Mul<Rhs> for Ratio<LhsNum,LhsDen>
where
LhsNum:core::ops::Mul<Rhs>,
{
type Output=Ratio<<LhsNum as core::ops::Mul<Rhs>>::Output,LhsDen>;
#[inline]
fn mul(self,rhs:Rhs)->Self::Output{
Ratio::new(self.num*rhs,self.den)
}
}
impl<LhsNum,LhsDen,Rhs> core::ops::Div<Rhs> for Ratio<LhsNum,LhsDen>
where
LhsDen:core::ops::Mul<Rhs>,
{
type Output=Ratio<LhsNum,<LhsDen as core::ops::Mul<Rhs>>::Output>;
#[inline]
fn div(self,rhs:Rhs)->Self::Output{
Ratio::new(self.num,self.den*rhs)
}
}
macro_rules! impl_ratio_operator {
($trait:ident, $method:ident) => {
impl<LhsNum,LhsDen,Rhs,Intermediate> core::ops::$trait<Rhs> for Ratio<LhsNum,LhsDen>
where
LhsNum:core::ops::$trait<Intermediate>,
LhsDen:Copy,
Rhs:core::ops::Mul<LhsDen,Output=Intermediate>,
{
type Output=Ratio<<LhsNum as core::ops::$trait<Intermediate>>::Output,LhsDen>;
#[inline]
fn $method(self,rhs:Rhs)->Self::Output{
Ratio::new(self.num.$method(rhs*self.den),self.den)
}
}
};
}
impl_ratio_operator!(Add,add);
impl_ratio_operator!(Sub,sub);
impl_ratio_operator!(Rem,rem);
//assign operators
impl<LhsNum,LhsDen,Rhs> core::ops::MulAssign<Rhs> for Ratio<LhsNum,LhsDen>
where
LhsNum:core::ops::MulAssign<Rhs>,
{
#[inline]
fn mul_assign(&mut self,rhs:Rhs){
self.num*=rhs;
}
}
impl<LhsNum,LhsDen,Rhs> core::ops::DivAssign<Rhs> for Ratio<LhsNum,LhsDen>
where
LhsDen:core::ops::MulAssign<Rhs>,
{
#[inline]
fn div_assign(&mut self,rhs:Rhs){
self.den*=rhs;
}
}
macro_rules! impl_ratio_assign_operator {
($trait:ident, $method:ident) => {
impl<LhsNum,LhsDen,Rhs> core::ops::$trait<Rhs> for Ratio<LhsNum,LhsDen>
where
LhsNum:core::ops::$trait,
LhsDen:Copy,
Rhs:core::ops::Mul<LhsDen,Output=LhsNum>,
{
#[inline]
fn $method(&mut self,rhs:Rhs){
self.num.$method(rhs*self.den)
}
}
};
}
impl_ratio_assign_operator!(AddAssign,add_assign);
impl_ratio_assign_operator!(SubAssign,sub_assign);
impl_ratio_assign_operator!(RemAssign,rem_assign);
// Only implement PartialEq<Self>
// Rust's operators aren't actually that good
impl<LhsNum,LhsDen,RhsNum,RhsDen,T,U> PartialEq<Ratio<RhsNum,RhsDen>> for Ratio<LhsNum,LhsDen>
where
LhsNum:Copy,
LhsDen:Copy,
RhsNum:Copy,
RhsDen:Copy,
LhsNum:core::ops::Mul<RhsDen,Output=T>,
RhsNum:core::ops::Mul<LhsDen,Output=U>,
T:PartialEq<U>,
{
#[inline]
fn eq(&self,other:&Ratio<RhsNum,RhsDen>)->bool{
(self.num*other.den).eq(&(other.num*self.den))
}
}
impl<Num,Den> Eq for Ratio<Num,Den> where Self:PartialEq{}
// Wow! These were both completely wrong!
// Idea: use a 'signed' trait instead of parity and float the sign to the numerator.
impl<LhsNum,LhsDen,RhsNum,RhsDen,T,U> PartialOrd<Ratio<RhsNum,RhsDen>> for Ratio<LhsNum,LhsDen>
where
LhsNum:Copy,
LhsDen:Copy+Parity,
RhsNum:Copy,
RhsDen:Copy+Parity,
LhsNum:core::ops::Mul<RhsDen,Output=T>,
LhsDen:core::ops::Mul<RhsNum,Output=T>,
RhsNum:core::ops::Mul<LhsDen,Output=U>,
RhsDen:core::ops::Mul<LhsNum,Output=U>,
T:PartialOrd<U>+Ord,
{
#[inline]
fn partial_cmp(&self,&other:&Ratio<RhsNum,RhsDen>)->Option<core::cmp::Ordering>{
self.partial_cmp_ratio(other)
}
}
impl<Num,Den,T> Ord for Ratio<Num,Den>
where
Num:Copy,
Den:Copy+Parity,
Num:core::ops::Mul<Den,Output=T>,
Den:core::ops::Mul<Num,Output=T>,
T:Ord,
{
#[inline]
fn cmp(&self,&other:&Self)->std::cmp::Ordering{
self.cmp_ratio(other)
}
}

View File

@@ -1,58 +0,0 @@
use crate::ratio::Ratio;
macro_rules! test_op{
($ratio_op:ident,$op:ident,$a:expr,$b:expr,$c:expr,$d:expr)=>{
assert_eq!(
Ratio::new($a,$b).$ratio_op(Ratio::new($c,$d)),
(($a as f32)/($b as f32)).$op(&(($c as f32)/($d as f32)))
);
};
}
macro_rules! test_many_ops{
($ratio_op:ident,$op:ident)=>{
test_op!($ratio_op,$op,1,2,3,4);
test_op!($ratio_op,$op,1,2,-3,4);
test_op!($ratio_op,$op,-1,2,-3,4);
test_op!($ratio_op,$op,-1,-2,-3,4);
test_op!($ratio_op,$op,2,1,6,3);
test_op!($ratio_op,$op,-2,1,6,3);
test_op!($ratio_op,$op,2,-1,-6,3);
test_op!($ratio_op,$op,2,1,6,-3);
};
}
#[test]
fn test_lt(){
test_many_ops!(lt_ratio,lt);
}
#[test]
fn test_gt(){
test_many_ops!(gt_ratio,gt);
}
#[test]
fn test_le(){
test_many_ops!(le_ratio,le);
}
#[test]
fn test_ge(){
test_many_ops!(ge_ratio,ge);
}
#[test]
fn test_eq(){
test_many_ops!(eq_ratio,eq);
}
#[test]
fn test_partial_cmp(){
test_many_ops!(partial_cmp_ratio,partial_cmp);
}
// #[test]
// fn test_cmp(){
// test_many_ops!(cmp_ratio,cmp);
// }

View File

@@ -88,6 +88,11 @@ pub enum Instruction{
PracticeFly,
#[brw(magic=14u8)]
SetSensitivity(super::integer::Ratio64Vec2),
#[brw(magic=15u8)]
SetSprint(
#[br(map=bool_from_u8)]
#[bw(map=bool_into_u8)]
bool),
#[brw(magic=255u8)]
Idle,
}
@@ -116,6 +121,7 @@ impl TryInto<strafesnet_common::physics::Instruction> for Instruction{
Instruction::SetMoveForward(state)=>strafesnet_common::physics::Instruction::SetControl(strafesnet_common::physics::SetControlInstruction::SetMoveForward(state.into())),
Instruction::SetJump(state)=>strafesnet_common::physics::Instruction::SetControl(strafesnet_common::physics::SetControlInstruction::SetJump(state.into())),
Instruction::SetZoom(state)=>strafesnet_common::physics::Instruction::SetControl(strafesnet_common::physics::SetControlInstruction::SetZoom(state.into())),
Instruction::SetSprint(state)=>strafesnet_common::physics::Instruction::SetControl(strafesnet_common::physics::SetControlInstruction::SetSprint(state.into())),
Instruction::Reset=>strafesnet_common::physics::Instruction::Mode(strafesnet_common::physics::ModeInstruction::Reset),
Instruction::Restart(mode_id)=>strafesnet_common::physics::Instruction::Mode(strafesnet_common::physics::ModeInstruction::Restart(strafesnet_common::gameplay_modes::ModeId::new(mode_id))),
Instruction::Spawn(mode_id,stage_id)=>strafesnet_common::physics::Instruction::Mode(strafesnet_common::physics::ModeInstruction::Spawn(
@@ -142,6 +148,7 @@ impl TryFrom<strafesnet_common::physics::Instruction> for Instruction{
strafesnet_common::physics::Instruction::SetControl(strafesnet_common::physics::SetControlInstruction::SetMoveForward(state))=>Ok(Instruction::SetMoveForward(state.into())),
strafesnet_common::physics::Instruction::SetControl(strafesnet_common::physics::SetControlInstruction::SetJump(state))=>Ok(Instruction::SetJump(state.into())),
strafesnet_common::physics::Instruction::SetControl(strafesnet_common::physics::SetControlInstruction::SetZoom(state))=>Ok(Instruction::SetZoom(state.into())),
strafesnet_common::physics::Instruction::SetControl(strafesnet_common::physics::SetControlInstruction::SetSprint(state))=>Ok(Instruction::SetSprint(state.into())),
strafesnet_common::physics::Instruction::Mode(strafesnet_common::physics::ModeInstruction::Reset)=>Ok(Instruction::Reset),
strafesnet_common::physics::Instruction::Mode(strafesnet_common::physics::ModeInstruction::Restart(mode_id))=>Ok(Instruction::Restart(mode_id.get())),
strafesnet_common::physics::Instruction::Mode(strafesnet_common::physics::ModeInstruction::Spawn(mode_id,stage_id))=>Ok(Instruction::Spawn(

View File

@@ -138,6 +138,7 @@ impl WindowContext<'_>{
if let Some(session_instruction)=match keycode{
winit::keyboard::Key::Named(winit::keyboard::NamedKey::Space)=>input_ctrl!(SetJump,s),
winit::keyboard::Key::Named(winit::keyboard::NamedKey::Shift)=>input_ctrl!(SetSprint,s),
// TODO: bind system so playback pausing can use spacebar
winit::keyboard::Key::Named(winit::keyboard::NamedKey::Enter)=>if s{
let paused=!self.simulation_paused;