Compare commits

..

28 Commits

Author SHA1 Message Date
6ca6e4ac3d stop being so wide 2024-09-26 18:46:04 -07:00
4eda90e178 floor(x+1)==floor(x)+1 2024-09-26 18:44:12 -07:00
72c91558e7 use one type def for dt ratio 2024-09-26 18:43:11 -07:00
9453ef17fb uniquely identify test 2024-09-26 18:04:58 -07:00
7425f1cd01 fix tests 2024-09-26 18:04:49 -07:00
682fa44741 fix fart 2024-09-26 16:39:32 -07:00
84b364313b debug things 2024-09-26 16:39:32 -07:00
6005485789 fix face_nd 2024-09-26 16:09:37 -07:00
2fc1fcfa80 Revert "det unused"
This reverts commit 6b897620e9.
2024-09-26 16:07:46 -07:00
8a1e713170 column major 2024-09-26 15:08:50 -07:00
a685c4ebe2 point to commit 2024-09-25 14:55:36 -07:00
0150bb977d optimize is_empty_volume 2024-09-25 10:18:38 -07:00
f280b65fc9 fix is_empty_volume test 2024-09-25 10:00:24 -07:00
327ba10fb4 already 8 2024-09-17 15:56:26 -07:00
6b897620e9 det unused 2024-09-17 15:06:12 -07:00
a5efbe6574 plumb time ratio everywhere and make a special spot for it in internal instructions 2024-09-16 15:05:11 -07:00
a64b81f05e precise time ratio 2024-09-16 15:04:38 -07:00
462950d526 ignore precision 2024-09-16 15:03:52 -07:00
c5f9a30c1b graphics 2024-09-16 15:03:18 -07:00
1d4a79b15e graphics: wip 2024-09-16 13:05:49 -07:00
4556bc8741 do face crawler 2024-09-16 13:05:09 -07:00
215dfab8a9 write body extrapolation from time ratio using ridiculous trait bounds 2024-09-16 13:05:09 -07:00
d6e15dea05 fix errors 2024-09-13 14:01:13 -07:00
ec8c89de9c many work 2024-09-13 13:51:33 -07:00
33c9fb0399 wip 2024-09-12 13:49:48 -07:00
bbc9a89ad7 physics.rs cleared 2024-09-12 13:49:42 -07:00
fc7d4b7e0c more wip 2024-09-12 13:12:19 -07:00
60bd926ce1 wip wip 2024-09-12 13:09:25 -07:00
8 changed files with 271 additions and 848 deletions

693
Cargo.lock generated

File diff suppressed because it is too large Load Diff

@ -1,6 +1,6 @@
[package]
name = "strafe-client"
version = "0.10.5"
version = "0.10.3"
edition = "2021"
repository = "https://git.itzana.me/StrafesNET/strafe-client"
license = "Custom"
@ -15,21 +15,20 @@ source = ["dep:strafesnet_deferred_loader", "dep:strafesnet_bsp_loader"]
roblox = ["dep:strafesnet_deferred_loader", "dep:strafesnet_rbx_loader"]
[dependencies]
arrayvec = "0.7.6"
bytemuck = { version = "1.13.1", features = ["derive"] }
configparser = "3.0.2"
ddsfile = "0.5.1"
glam = "0.29.0"
glam = "0.28.0"
id = { version = "0.1.0", registry = "strafesnet" }
parking_lot = "0.12.1"
pollster = "0.3.0"
strafesnet_bsp_loader = { version = "0.2.1", registry = "strafesnet", optional = true }
strafesnet_common = { version = "0.5.2", registry = "strafesnet" }
strafesnet_deferred_loader = { version = "0.4.0", features = ["legacy"], registry = "strafesnet", optional = true }
strafesnet_rbx_loader = { version = "0.4.1", registry = "strafesnet", optional = true }
strafesnet_snf = { version = "0.2.0", registry = "strafesnet", optional = true }
wgpu = "22.1.0"
winit = "0.30.5"
strafesnet_bsp_loader = { version = "0.1.3", registry = "strafesnet", optional = true }
strafesnet_common = { git = "https://git.itzana.me/StrafesNET/common", rev = "c5f01d4c3ea3aa2ee3448ff42f020f5f5239dc95" }
strafesnet_deferred_loader = { version = "0.3.1", features = ["legacy"], registry = "strafesnet", optional = true }
strafesnet_rbx_loader = { version = "0.3.2", registry = "strafesnet", optional = true }
strafesnet_snf = { git = "https://git.itzana.me/StrafesNET/snf", rev = "1e0095ed1583f0330d8dbdf1f56edd61bfb43eec", optional = true }
wgpu = "22.0.0"
winit = "0.30.4"
[profile.release]
#lto = true

@ -15,7 +15,7 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
fn next_transition(fev:&MinkowskiFEV,body_time:GigaTime,mesh:&MinkowskiMesh,body:&Body,mut best_time:GigaTime)->MinkowskiTransition{
//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=MinkowskiTransition::Miss;
let mut best_transtition=MinkowskiTransition::Miss;
match fev{
&MinkowskiFEV::Face(face_id)=>{
//test own face collision time, ignoring roots with zero or conflicting derivative
@ -24,10 +24,10 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
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)){
for dt in Fixed::<2,64>::zeroes2((n.dot(body.position)-d).fix_2()*2,n.dot(body.velocity).fix_2()*2,n.dot(body.acceleration).fix_2()){
if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
best_time=dt;
best_transition=MinkowskiTransition::Hit(face_id,dt);
best_transtition=MinkowskiTransition::Hit(face_id,dt);
break;
}
}
@ -38,10 +38,10 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
let verts=mesh.edge_verts(directed_edge_id.as_undirected());
//WARNING: d is moved out of the *2 block because of adding two vertices!
//WARNING: precision is swept under the rug!
for dt in Fixed::<4,128>::zeroes2(n.dot(body.position*2-(mesh.vert(verts[0])+mesh.vert(verts[1]))).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){
for dt in Fixed::<2,64>::zeroes2(n.dot(body.position*2-(mesh.vert(verts[0])+mesh.vert(verts[1]))).fix_2(),n.dot(body.velocity).fix_2()*2,n.dot(body.acceleration).fix_2()){
if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
best_time=dt;
best_transition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt);
best_transtition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt);
break;
}
}
@ -58,10 +58,10 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
//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
for dt in Fixed::<4,128>::zeroes2(n.dot(delta_pos).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){
for dt in Fixed::<2,64>::zeroes2(n.dot(delta_pos).fix_2(),n.dot(body.velocity).fix_2()*2,n.dot(body.acceleration).fix_2()){
if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
best_time=dt;
best_transition=MinkowskiTransition::Next(MinkowskiFEV::Face(edge_face_id),dt);
best_transtition=MinkowskiTransition::Next(MinkowskiFEV::Face(edge_face_id),dt);
break;
}
}
@ -72,9 +72,9 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
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 body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4());
let dt=Ratio::new(dt.num.fix_2(),dt.den.fix_2());
best_time=dt;
best_transition=MinkowskiTransition::Next(MinkowskiFEV::Vert(vert_id),dt);
best_transtition=MinkowskiTransition::Next(MinkowskiFEV::Vert(vert_id),dt);
break;
}
}
@ -88,9 +88,9 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
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 body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4());
let dt=Ratio::new(dt.num.fix_2(),dt.den.fix_2());
best_time=dt;
best_transition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt);
best_transtition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt);
break;
}
}
@ -98,7 +98,7 @@ type MinkowskiTransition=Transition<MinkowskiFace,MinkowskiDirectedEdge,Minkowsk
//if none:
},
}
best_transition
best_transtition
}
pub enum CrawlResult<F,E:DirectedEdge,V>{
Miss(FEV<F,E,V>),
@ -108,11 +108,11 @@ type MinkowskiCrawlResult=CrawlResult<MinkowskiFace,MinkowskiDirectedEdge,Minkow
pub fn crawl_fev(mut fev:MinkowskiFEV,mesh:&MinkowskiMesh,relative_body:&Body,start_time:Time,time_limit:Time)->MinkowskiCrawlResult{
let mut body_time={
let r=(start_time-relative_body.time).to_ratio();
Ratio::new(r.num.fix_4(),r.den.fix_4())
Ratio::new(r.num.fix_2(),r.den.fix_2())
};
let time_limit={
let r=(time_limit-relative_body.time).to_ratio();
Ratio::new(r.num.fix_4(),r.den.fix_4())
Ratio::new(r.num.fix_2(),r.den.fix_2())
};
for _ in 0..20{
match next_transition(&fev,body_time,mesh,relative_body,time_limit){

@ -66,9 +66,7 @@ pub fn load<P:AsRef<std::path::Path>>(path:P)->Result<strafesnet_common::map::Co
#[cfg(feature="snf")]
DataStructure::StrafesNET(map)=>Ok(map),
#[cfg(feature="roblox")]
DataStructure::Roblox(mut dom)=>{
dom.run_scripts();
DataStructure::Roblox(dom)=>{
let mut loader=strafesnet_deferred_loader::roblox_legacy();
let (texture_loader,mesh_loader)=loader.get_inner_mut();
@ -112,7 +110,7 @@ pub fn load<P:AsRef<std::path::Path>>(path:P)->Result<strafesnet_common::map::Co
|name|mesh_loader.acquire_mesh_id(name),
);
let prop_meshes=mesh_loader.load_meshes(bsp.as_ref());
let prop_meshes=mesh_loader.load_meshes(&bsp.as_ref());
let map_step2=map_step1.add_prop_meshes(
//the type conflagulator 9000

@ -5,7 +5,6 @@ mod worker;
mod physics;
mod graphics;
mod settings;
mod push_solve;
mod face_crawler;
mod compat_worker;
mod model_physics;

@ -599,7 +599,7 @@ enum EV{
Edge(MinkowskiEdge),
}
pub type GigaTime=Ratio<Fixed<4,128>,Fixed<4,128>>;
pub type GigaTime=Ratio<Fixed<2,64>,Fixed<2,64>>;
impl MinkowskiMesh<'_>{
pub fn minkowski_sum<'a>(mesh0:TransformedMesh<'a>,mesh1:TransformedMesh<'a>)->MinkowskiMesh<'a>{
@ -707,8 +707,7 @@ impl MinkowskiMesh<'_>{
infinity_body.velocity=dir;
infinity_body.acceleration=vec3::ZERO;
//crawl in from negative infinity along a tangent line to get the closest fev
// TODO: change crawl_fev args to delta time? Optional values?
match crate::face_crawler::crawl_fev(infinity_fev,self,&infinity_body,integer::Time::MIN/4,infinity_body.time){
match crate::face_crawler::crawl_fev(infinity_fev,self,&infinity_body,integer::Time::MIN,infinity_body.time){
crate::face_crawler::CrawlResult::Miss(fev)=>Some(fev),
crate::face_crawler::CrawlResult::Hit(_,_)=>None,
}
@ -744,7 +743,7 @@ impl MinkowskiMesh<'_>{
//determine when it passes an edge ("sliding off" case)
let mut best_time={
let r=(time_limit-relative_body.time).to_ratio();
Ratio::new(r.num.fix_4(),r.den.fix_4())
Ratio::new(r.num.fix_2(),r.den.fix_2())
};
let mut best_edge=None;
let face_n=self.face_nd(contact_face_id).0;
@ -756,8 +755,8 @@ impl MinkowskiMesh<'_>{
let d=n.dot(self.vert(verts[0])+self.vert(verts[1]));
//WARNING! d outside of *2
//WARNING: truncated precision
for dt in Fixed::<4,128>::zeroes2(((n.dot(relative_body.position))*2-d).fix_4(),n.dot(relative_body.velocity).fix_4()*2,n.dot(relative_body.acceleration).fix_4()){
if Ratio::new(Planar64::ZERO,Planar64::EPSILON).le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(relative_body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
for dt in Fixed::<2,64>::zeroes2(((n.dot(relative_body.position))*2-d).fix_2(),n.dot(relative_body.velocity).fix_2()*2,n.dot(relative_body.acceleration).fix_2()){
if dt.num.is_positive()&&dt.lt_ratio(best_time)&&n.dot(relative_body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
best_time=dt;
best_edge=Some(directed_edge_id);
break;
@ -768,7 +767,7 @@ impl MinkowskiMesh<'_>{
}
fn infinity_in(&self,infinity_body:crate::physics::Body)->Option<(MinkowskiFace,GigaTime)>{
let infinity_fev=self.infinity_fev(-infinity_body.velocity,infinity_body.position);
match crate::face_crawler::crawl_fev(infinity_fev,self,&infinity_body,integer::Time::MIN/4,infinity_body.time){
match crate::face_crawler::crawl_fev(infinity_fev,self,&infinity_body,integer::Time::MIN,infinity_body.time){
crate::face_crawler::CrawlResult::Miss(_)=>None,
crate::face_crawler::CrawlResult::Hit(face,time)=>Some((face,time)),
}
@ -850,7 +849,7 @@ impl MeshQuery<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert> for MinkowskiM
let &[e1f0,e1f1]=self.mesh1.edge_faces(e1).borrow();
Cow::Owned([(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 mut best_d:Ratio<Fixed<2,64>,Fixed<2,64>>=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.iter(){
@ -861,7 +860,7 @@ impl MeshQuery<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert> for MinkowskiM
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);
let dd=(d*d).fix_2()/(edge_face1_nn*edge0_nn).fix_2();
if best_d<dd{
best_d=dd;
best_edge=Some(directed_edge_id0);
@ -880,7 +879,7 @@ impl MeshQuery<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert> for MinkowskiM
let &[e0f0,e0f1]=self.mesh0.edge_faces(e0).borrow();
Cow::Owned([(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 mut best_d:Ratio<Fixed<2,64>,Fixed<2,64>>=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.iter(){
@ -888,7 +887,7 @@ impl MeshQuery<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert> for MinkowskiM
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);
let dd=(d*d).fix_2()/(edge_face0_nn*edge1_nn).fix_2();
if best_d<dd{
best_d=dd;
best_edge=Some(directed_edge_id1);

@ -766,31 +766,23 @@ impl TouchingState{
a
}
fn constrain_velocity(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,velocity:&mut Planar64Vec3){
let contacts=self.contacts.iter().map(|contact|{
//TODO: trey push solve
for contact in &self.contacts{
let n=contact_normal(models,hitbox_mesh,contact);
crate::push_solve::Contact{
position:Planar64Vec3::ZERO,
velocity:n,
normal:n,
let d=n.dot(*velocity);
if d.is_negative(){
*velocity-=(n*d/n.length_squared()).divide().fix_1();
}
}).collect();
match crate::push_solve::push_solve(&contacts,*velocity){
Some(new_velocity)=>*velocity=new_velocity,
None=>println!("Algorithm silently failing :)"),
}
}
fn constrain_acceleration(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,acceleration:&mut Planar64Vec3){
let contacts=self.contacts.iter().map(|contact|{
//TODO: trey push solve
for contact in &self.contacts{
let n=contact_normal(models,hitbox_mesh,contact);
crate::push_solve::Contact{
position:Planar64Vec3::ZERO,
velocity:n,
normal:n,
let d=n.dot(*acceleration);
if d.is_negative(){
*acceleration-=(n*d/n.length_squared()).divide().fix_1();
}
}).collect();
match crate::push_solve::push_solve(&contacts,*acceleration){
Some(new_acceleration)=>*acceleration=new_acceleration,
None=>println!("Algorithm silently failing :)"),
}
}
fn predict_collision_end(&self,collector:&mut instruction::InstructionCollector<PhysicsInternalInstruction>,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,body:&Body,time:Time){
@ -1965,7 +1957,7 @@ fn atomic_input_instruction(state:&mut PhysicsState,data:&PhysicsData,ins:TimedI
_=>println!("{}|{:?}",ins.time,ins.instruction),
}
if ins.time<state.time{
println!("@@@@ Time travel warning! state.time={} ins.time={}\nInstruction={:?}",state.time,ins.time,ins.instruction);
println!("@@@@ Time travel warning! {:?}",ins);
}
//idle is special, it is specifically a no-op to get Internal events to catch up to real time
match ins.instruction{

@ -1,321 +0,0 @@
use strafesnet_common::integer::{vec3::{self, Vector3}, Fixed, Planar64, Planar64Vec3, Ratio};
// This algorithm is based on Lua code
// written by Trey Reynolds in 2021
// 1/2^10
const EPSILON:Planar64=Planar64::raw(1<<(32-10));
// A stack-allocated variable-size list that holds up to 4 elements
// Direct references are used instead of indices i0, i1, i2, i3
type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>;
struct Ray{
origin:Planar64Vec3,
direction:Planar64Vec3,
}
impl Ray{
fn extrapolate(&self,t:Planar64)->Planar64Vec3{
self.origin+self.direction*t
}
}
/// Information about a contact restriction
pub struct Contact{
pub position:Planar64Vec3,
pub velocity:Planar64Vec3,
pub normal:Planar64Vec3,
}
impl Contact{
fn relative_to(&self,point:Planar64Vec3)->Self{
Self{
position:self.position-point,
velocity:self.velocity,
normal:self.normal,
}
}
fn relative_dot(&self,direction:Planar64Vec3)->Fixed<2,64>{
(direction-self.velocity).dot(self.normal)
}
/// Calculate the time of intersection. (previously get_touch_time)
fn solve(&self,ray:&Ray)->Ratio<Fixed<2,64>,Fixed<2,64>>{
(self.position-ray.origin).dot(self.normal)/(ray.direction-self.velocity).dot(self.normal)
}
}
//note that this is horrible with fixed point arithmetic
fn solve1(c0:&Contact)->Ratio<Vector3<Fixed<3,96>>,Fixed<2,64>>{
let det=c0.normal.dot(c0.velocity);
let d0=c0.normal.dot(c0.position);
c0.normal*d0/det
}
fn solve2(c0:&Contact,c1:&Contact)->Ratio<Vector3<Fixed<5,160>>,Fixed<4,128>>{
let u0_u1=c0.velocity.cross(c1.velocity);
let n0_n1=c0.normal.cross(c1.normal);
let det=u0_u1.dot(n0_n1);
let d0=c0.normal.dot(c0.position);
let d1=c1.normal.dot(c1.position);
(c1.normal.cross(u0_u1)*d0+u0_u1.cross(c0.normal)*d1)/det
}
fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Ratio<Vector3<Fixed<4,128>>,Fixed<3,96>>{
let n0_n1=c0.normal.cross(c1.normal);
let det=c2.normal.dot(n0_n1);
let d0=c0.normal.dot(c0.position);
let d1=c1.normal.dot(c1.position);
let d2=c2.normal.dot(c2.position);
(c1.normal.cross(c2.normal)*d0+c2.normal.cross(c0.normal)*d1+c0.normal.cross(c1.normal)*d2)/det
}
fn decompose1(point:Planar64Vec3,u0:Planar64Vec3)->[Ratio<Fixed<2,64>,Fixed<2,64>>;1]{
let det=u0.dot(u0);
let s0=u0.dot(point)/det;
[s0]
}
fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->[Ratio<Fixed<4,128>,Fixed<4,128>>;2]{
let u0_u1=u0.cross(u1);
let det=u0_u1.dot(u0_u1);
let s0=u0_u1.dot(point.cross(u1))/det;
let s1=u0_u1.dot(u0.cross(point))/det;
[s0,s1]
}
fn decompose3(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3,u2:Planar64Vec3)->[Ratio<Fixed<3,96>,Fixed<3,96>>;3]{
let det=u0.cross(u1).dot(u2);
let s0=point.cross(u1).dot(u2)/det;
let s1=u0.cross(point).dot(u2)/det;
let s2=u0.cross(u1).dot(point)/det;
[s0,s1,s2]
}
fn is_space_enclosed_2(
a:Planar64Vec3,
b:Planar64Vec3,
)->bool{
a.cross(b)==Vector3::new([Fixed::ZERO;3])
&&a.dot(b).is_negative()
}
fn is_space_enclosed_3(
a:Planar64Vec3,
b:Planar64Vec3,
c:Planar64Vec3
)->bool{
a.cross(b).dot(c)==Fixed::ZERO
&&{
let det_abac=a.cross(b).dot(a.cross(c));
let det_abbc=a.cross(b).dot(b.cross(c));
let det_acbc=a.cross(c).dot(b.cross(c));
return!( det_abac*det_abbc).is_positive()
&&!( det_abbc*det_acbc).is_positive()
&&!(-det_acbc*det_abac).is_positive()
||is_space_enclosed_2(a,b)
||is_space_enclosed_2(a,c)
||is_space_enclosed_2(b,c)
}
}
fn is_space_enclosed_4(
a:Planar64Vec3,
b:Planar64Vec3,
c:Planar64Vec3,
d:Planar64Vec3,
)->bool{
let det_abc=a.cross(b).dot(c);
let det_abd=a.cross(b).dot(d);
let det_acd=a.cross(c).dot(d);
let det_bcd=b.cross(c).dot(d);
return( det_abc*det_abd).is_negative()
&&(-det_abc*det_acd).is_negative()
&&( det_abd*det_acd).is_negative()
&&( det_abc*det_bcd).is_negative()
&&(-det_abd*det_bcd).is_negative()
&&( det_acd*det_bcd).is_negative()
||is_space_enclosed_3(a,b,c)
||is_space_enclosed_3(a,b,d)
||is_space_enclosed_3(a,c,d)
||is_space_enclosed_3(b,c,d)
}
const fn get_push_ray_0(point:Planar64Vec3)->Option<Ray>{
Some(Ray{origin:point,direction:vec3::ZERO})
}
fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option<Ray>{
let direction=solve1(c0);
let s0=decompose1(direction,c0.velocity);
if s0<Planar64::ZERO{
return None;
}
let origin=point+solve1(
&c0.relative_to(point),
).divide().fix_1();
Some(Ray{origin,direction})
}
fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option<Ray>{
let direction=solve2(c0,c1)?;
let (s0,s1)=decompose2(direction,c0.velocity,c1.velocity)?;
if s0<Planar64::ZERO||s1<Planar64::ZERO{
return None;
}
let origin=point+solve2(
&c0.relative_to(point),
&c1.relative_to(point),
)?;
Some(Ray{origin,direction})
}
fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Option<Ray>{
let direction=solve3(c0,c1,c2)?;
let (s0,s1,s2)=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?;
if s0<Planar64::ZERO||s1<Planar64::ZERO||s2<Planar64::ZERO{
return None;
}
let origin=point+solve3(
&c0.relative_to(point),
&c1.relative_to(point),
&c2.relative_to(point),
)?;
Some(Ray{origin,direction})
}
const fn get_best_push_ray_and_conts_0<'a>(point:Planar64Vec3)->Option<(Ray,Conts<'a>)>{
match get_push_ray_0(point){
Some(ray)=>Some((ray,Conts::new_const())),
None=>None,
}
}
fn get_best_push_ray_and_conts_1(point:Planar64Vec3,c0:&Contact)->Option<(Ray,Conts)>{
get_push_ray_1(point,c0)
.map(|ray|(ray,Conts::from_iter([c0])))
}
fn get_best_push_ray_and_conts_2<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact)->Option<(Ray,Conts<'a>)>{
if is_space_enclosed_2(c0.normal,c1.normal){
return None;
}
if let Some(ray)=get_push_ray_2(point,c0,c1){
return Some((ray,Conts::from_iter([c0,c1])));
}
if let Some(ray)=get_push_ray_1(point,c0){
if !c1.relative_dot(ray.direction).is_negative(){
return Some((ray,Conts::from_iter([c0])));
}
}
return None;
}
fn get_best_push_ray_and_conts_3<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact)->Option<(Ray,Conts<'a>)>{
if is_space_enclosed_3(c0.normal,c1.normal,c2.normal){
return None;
}
if let Some(ray)=get_push_ray_3(point,c0,c1,c2){
return Some((ray,Conts::from_iter([c0,c1,c2])));
}
if let Some(ray)=get_push_ray_2(point,c0,c1){
if !c2.relative_dot(ray.direction).is_negative(){
return Some((ray,Conts::from_iter([c0,c1])));
}
}
if let Some(ray)=get_push_ray_2(point,c0,c2){
if !c1.relative_dot(ray.direction).is_negative(){
return Some((ray,Conts::from_iter([c0,c2])));
}
}
if let Some(ray)=get_push_ray_1(point,c0){
if !c1.relative_dot(ray.direction).is_negative()
&&!c2.relative_dot(ray.direction).is_negative(){
return Some((ray,Conts::from_iter([c0])));
}
}
return None;
}
fn get_best_push_ray_and_conts_4<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact,c3:&'a Contact)->Option<(Ray,Conts<'a>)>{
if is_space_enclosed_4(c0.normal,c1.normal,c2.normal,c3.normal){
return None;
}
let (ray012,conts012)=get_best_push_ray_and_conts_3(point,c0,c1,c2)?;
let (ray013,conts013)=get_best_push_ray_and_conts_3(point,c0,c1,c3)?;
let (ray023,conts023)=get_best_push_ray_and_conts_3(point,c0,c2,c3)?;
let err012=c3.relative_dot(ray012.direction);
let err013=c2.relative_dot(ray013.direction);
let err023=c1.relative_dot(ray023.direction);
let best_err=err012.max(err013).max(err023);
if best_err==err012{
return Some((ray012,conts012))
}else if best_err==err013{
return Some((ray013,conts013))
}else if best_err==err023{
return Some((ray023,conts023))
}
unreachable!()
}
fn get_best_push_ray_and_conts<'a>(
point:Planar64Vec3,
conts:Conts<'a>,
)->Option<(Ray,Conts<'a>)>{
match conts.as_slice(){
&[c0,c1,c2,c3]=>get_best_push_ray_and_conts_4(point,c0,c1,c2,c3),
&[c0,c1,c2]=>get_best_push_ray_and_conts_3(point,c0,c1,c2),
&[c0,c1]=>get_best_push_ray_and_conts_2(point,c0,c1),
&[c0]=>get_best_push_ray_and_conts_1(point,c0),
&[]=>get_best_push_ray_and_conts_0(point),
_=>unreachable!(),
}
}
fn get_first_touch<'a>(contacts:&'a Vec<Contact>,ray:&Ray,conts:&Conts)->Option<(Ratio<Fixed<2,64>,Fixed<2,64>>,&'a Contact)>{
contacts.iter()
.filter(|&contact|
!conts.iter().any(|&c|std::ptr::eq(c,contact))
&&contact.relative_dot(ray.direction).is_negative()
)
.map(|contact|(contact.solve(ray),contact))
.min_by_key(|&(t,_)|t)
}
pub fn push_solve(contacts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
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,conts))=>(t,conts),
None=>return Some(ray.origin),
};
if Planar64::ZERO<=next_t{
return Some(ray.origin);
}
//push_front
if conts.len()==conts.capacity(){
//this is a dead case, new_conts never has more than 3 elements
conts.rotate_right(1);
conts[0]=next_cont;
}else{
conts.push(next_cont);
conts.rotate_right(1);
}
let meet_point=ray.extrapolate(next_t);
match get_best_push_ray_and_conts(meet_point,conts){
Some((new_ray,new_conts))=>(ray,conts)=(new_ray,new_conts),
None=>return Some(meet_point),
}
}
}
#[cfg(test)]
mod tests{
use super::*;
#[test]
fn test_push_solve(){
let contacts=vec![
Contact{
position:vec3::ZERO,
velocity:vec3::Y,
normal:vec3::Y,
}
];
assert_eq!(
Some(vec3::ZERO),
push_solve(&contacts,vec3::NEG_Y)
);
}
}