Compare commits
1 Commits
push-solve
...
run-timer-
| Author | SHA1 | Date | |
|---|---|---|---|
| ed69cf2297 |
5
Cargo.lock
generated
5
Cargo.lock
generated
@@ -105,9 +105,9 @@ checksum = "9d151e35f61089500b617991b791fc8bfd237ae50cd5950803758a179b41e67a"
|
|||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "arrayvec"
|
name = "arrayvec"
|
||||||
version = "0.7.6"
|
version = "0.7.4"
|
||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "7c02d123df017efcdfbd739ef81735b36c5ba83ec3c59c80a9d7ecc718f92e50"
|
checksum = "96d30a06541fbafbc7f82ed10c06164cfbd2c401138f6addd8404629c4b16711"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "as-raw-xcb-connection"
|
name = "as-raw-xcb-connection"
|
||||||
@@ -1882,7 +1882,6 @@ checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f"
|
|||||||
name = "strafe-client"
|
name = "strafe-client"
|
||||||
version = "0.10.3"
|
version = "0.10.3"
|
||||||
dependencies = [
|
dependencies = [
|
||||||
"arrayvec",
|
|
||||||
"bytemuck",
|
"bytemuck",
|
||||||
"configparser",
|
"configparser",
|
||||||
"ddsfile",
|
"ddsfile",
|
||||||
|
|||||||
@@ -15,7 +15,6 @@ source = ["dep:strafesnet_deferred_loader", "dep:strafesnet_bsp_loader"]
|
|||||||
roblox = ["dep:strafesnet_deferred_loader", "dep:strafesnet_rbx_loader"]
|
roblox = ["dep:strafesnet_deferred_loader", "dep:strafesnet_rbx_loader"]
|
||||||
|
|
||||||
[dependencies]
|
[dependencies]
|
||||||
arrayvec = "0.7.6"
|
|
||||||
bytemuck = { version = "1.13.1", features = ["derive"] }
|
bytemuck = { version = "1.13.1", features = ["derive"] }
|
||||||
configparser = "3.0.2"
|
configparser = "3.0.2"
|
||||||
ddsfile = "0.5.1"
|
ddsfile = "0.5.1"
|
||||||
|
|||||||
@@ -102,6 +102,7 @@ pub struct FrameState{
|
|||||||
pub body:crate::physics::Body,
|
pub body:crate::physics::Body,
|
||||||
pub camera:crate::physics::PhysicsCamera,
|
pub camera:crate::physics::PhysicsCamera,
|
||||||
pub time:integer::Time,
|
pub time:integer::Time,
|
||||||
|
pub run:strafesnet_common::run::Run,
|
||||||
}
|
}
|
||||||
|
|
||||||
pub struct GraphicsState{
|
pub struct GraphicsState{
|
||||||
|
|||||||
@@ -5,7 +5,6 @@ mod worker;
|
|||||||
mod physics;
|
mod physics;
|
||||||
mod graphics;
|
mod graphics;
|
||||||
mod settings;
|
mod settings;
|
||||||
mod push_solve;
|
|
||||||
mod face_crawler;
|
mod face_crawler;
|
||||||
mod compat_worker;
|
mod compat_worker;
|
||||||
mod model_physics;
|
mod model_physics;
|
||||||
|
|||||||
@@ -763,31 +763,23 @@ impl TouchingState{
|
|||||||
a
|
a
|
||||||
}
|
}
|
||||||
fn constrain_velocity(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,velocity:&mut Planar64Vec3){
|
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);
|
let n=contact_normal(models,hitbox_mesh,contact);
|
||||||
crate::push_solve::Contact{
|
let d=n.dot128(*velocity);
|
||||||
position:Planar64Vec3::ZERO,
|
if d<0{
|
||||||
velocity:n,
|
*velocity-=n*Planar64::raw(((d<<32)/n.dot128(n)) as i64);
|
||||||
normal:n,
|
|
||||||
}
|
}
|
||||||
}).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){
|
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);
|
let n=contact_normal(models,hitbox_mesh,contact);
|
||||||
crate::push_solve::Contact{
|
let d=n.dot128(*acceleration);
|
||||||
position:Planar64Vec3::ZERO,
|
if d<0{
|
||||||
velocity:n,
|
*acceleration-=n*Planar64::raw(((d<<32)/n.dot128(n)) as i64);
|
||||||
normal:n,
|
|
||||||
}
|
}
|
||||||
}).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){
|
fn predict_collision_end(&self,collector:&mut instruction::InstructionCollector<PhysicsInternalInstruction>,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,body:&Body,time:Time){
|
||||||
@@ -1054,6 +1046,9 @@ impl PhysicsContext{
|
|||||||
pub const fn get_next_mouse(&self)->&MouseState{
|
pub const fn get_next_mouse(&self)->&MouseState{
|
||||||
self.state.input_state.get_next_mouse()
|
self.state.input_state.get_next_mouse()
|
||||||
}
|
}
|
||||||
|
pub const fn run(&self)->run::Run{
|
||||||
|
self.state.run
|
||||||
|
}
|
||||||
/// use with caution, this is the only non-instruction way to mess with physics
|
/// use with caution, this is the only non-instruction way to mess with physics
|
||||||
pub fn generate_models(&mut self,map:&map::CompleteMap){
|
pub fn generate_models(&mut self,map:&map::CompleteMap){
|
||||||
self.state.clear();
|
self.state.clear();
|
||||||
|
|||||||
@@ -184,6 +184,7 @@ impl MouseInterpolator{
|
|||||||
crate::graphics::FrameState{
|
crate::graphics::FrameState{
|
||||||
body:self.physics.camera_body(),
|
body:self.physics.camera_body(),
|
||||||
camera:self.physics.camera(),
|
camera:self.physics.camera(),
|
||||||
|
run:self.physics.run(),
|
||||||
time:self.timer.time(time),
|
time:self.timer.time(time),
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,339 +0,0 @@
|
|||||||
use strafesnet_common::integer::{Planar64,Planar64Vec3};
|
|
||||||
|
|
||||||
// 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)->Planar64{
|
|
||||||
(direction-self.velocity).dot(self.normal)
|
|
||||||
}
|
|
||||||
/// Calculate the time of intersection. (previously get_touch_time)
|
|
||||||
fn solve(&self,ray:&Ray)->Planar64{
|
|
||||||
(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)->Option<Planar64Vec3>{
|
|
||||||
let det=c0.normal.dot(c0.velocity);
|
|
||||||
if det.get().abs()<EPSILON.get(){
|
|
||||||
return None;
|
|
||||||
}
|
|
||||||
let d0=c0.normal.dot(c0.position);
|
|
||||||
Some(c0.normal*d0/det)
|
|
||||||
}
|
|
||||||
fn solve2(c0:&Contact,c1:&Contact)->Option<Planar64Vec3>{
|
|
||||||
let u0_u1=c0.velocity.cross(c1.velocity);
|
|
||||||
let n0_n1=c0.normal.cross(c1.normal);
|
|
||||||
let det=u0_u1.dot(n0_n1);
|
|
||||||
if det.get().abs()<EPSILON.get(){
|
|
||||||
return None;
|
|
||||||
}
|
|
||||||
let d0=c0.normal.dot(c0.position);
|
|
||||||
let d1=c1.normal.dot(c1.position);
|
|
||||||
Some((c1.normal.cross(u0_u1)*d0+u0_u1.cross(c0.normal)*d1)/det)
|
|
||||||
}
|
|
||||||
fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Option<Planar64Vec3>{
|
|
||||||
let n0_n1=c0.normal.cross(c1.normal);
|
|
||||||
let det=c2.normal.dot(n0_n1);
|
|
||||||
if det.get().abs()<EPSILON.get(){
|
|
||||||
return None;
|
|
||||||
}
|
|
||||||
let d0=c0.normal.dot(c0.position);
|
|
||||||
let d1=c1.normal.dot(c1.position);
|
|
||||||
let d2=c2.normal.dot(c2.position);
|
|
||||||
Some((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)->Option<Planar64>{
|
|
||||||
let det=u0.dot(u0);
|
|
||||||
if det==Planar64::ZERO{
|
|
||||||
return None;
|
|
||||||
}
|
|
||||||
let s0=u0.dot(point)/det;
|
|
||||||
Some(s0)
|
|
||||||
}
|
|
||||||
fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->Option<(Planar64,Planar64)>{
|
|
||||||
let u0_u1=u0.cross(u1);
|
|
||||||
let det=u0_u1.dot(u0_u1);
|
|
||||||
if det==Planar64::ZERO{
|
|
||||||
return None;
|
|
||||||
}
|
|
||||||
let s0=u0_u1.dot(point.cross(u1))/det;
|
|
||||||
let s1=u0_u1.dot(u0.cross(point))/det;
|
|
||||||
Some((s0,s1))
|
|
||||||
}
|
|
||||||
fn decompose3(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3,u2:Planar64Vec3)->Option<(Planar64,Planar64,Planar64)>{
|
|
||||||
let det=u0.cross(u1).dot(u2);
|
|
||||||
if det==Planar64::ZERO{
|
|
||||||
return None;
|
|
||||||
}
|
|
||||||
let s0=point.cross(u1).dot(u2)/det;
|
|
||||||
let s1=u0.cross(point).dot(u2)/det;
|
|
||||||
let s2=u0.cross(u1).dot(point)/det;
|
|
||||||
Some((s0,s1,s2))
|
|
||||||
}
|
|
||||||
|
|
||||||
fn is_space_enclosed_2(
|
|
||||||
a:Planar64Vec3,
|
|
||||||
b:Planar64Vec3,
|
|
||||||
)->bool{
|
|
||||||
a.cross(b)==Planar64Vec3::ZERO
|
|
||||||
&&a.dot(b)<Planar64::ZERO
|
|
||||||
}
|
|
||||||
fn is_space_enclosed_3(
|
|
||||||
a:Planar64Vec3,
|
|
||||||
b:Planar64Vec3,
|
|
||||||
c:Planar64Vec3
|
|
||||||
)->bool{
|
|
||||||
a.cross(b).dot(c)==Planar64::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<=Planar64::ZERO
|
|
||||||
&& det_abbc*det_acbc<=Planar64::ZERO
|
|
||||||
&&-det_acbc*det_abac<=Planar64::ZERO
|
|
||||||
||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<Planar64::ZERO
|
|
||||||
&&-det_abc*det_acd<Planar64::ZERO
|
|
||||||
&& det_abd*det_acd<Planar64::ZERO
|
|
||||||
&& det_abc*det_bcd<Planar64::ZERO
|
|
||||||
&&-det_abd*det_bcd<Planar64::ZERO
|
|
||||||
&& det_acd*det_bcd<Planar64::ZERO
|
|
||||||
||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:Planar64Vec3::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),
|
|
||||||
)?;
|
|
||||||
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 Planar64::ZERO<=c1.relative_dot(ray.direction){
|
|
||||||
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 Planar64::ZERO<=c2.relative_dot(ray.direction){
|
|
||||||
return Some((ray,Conts::from_iter([c0,c1])));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if let Some(ray)=get_push_ray_2(point,c0,c2){
|
|
||||||
if Planar64::ZERO<=c1.relative_dot(ray.direction){
|
|
||||||
return Some((ray,Conts::from_iter([c0,c2])));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if let Some(ray)=get_push_ray_1(point,c0){
|
|
||||||
if Planar64::ZERO<=c1.relative_dot(ray.direction)
|
|
||||||
&&Planar64::ZERO<=c2.relative_dot(ray.direction){
|
|
||||||
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<(Planar64,&'a Contact)>{
|
|
||||||
contacts.iter()
|
|
||||||
.filter(|&contact|
|
|
||||||
!conts.iter().any(|&c|std::ptr::eq(c,contact))
|
|
||||||
&&contact.relative_dot(ray.direction)< -EPSILON
|
|
||||||
)
|
|
||||||
.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:Planar64Vec3::ZERO,
|
|
||||||
velocity:Planar64Vec3::Y,
|
|
||||||
normal:Planar64Vec3::Y,
|
|
||||||
}
|
|
||||||
];
|
|
||||||
assert_eq!(
|
|
||||||
Some(Planar64Vec3::ZERO),
|
|
||||||
push_solve(&contacts,Planar64Vec3::NEG_Y)
|
|
||||||
);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user