move engine components into modules
This commit is contained in:
349
engine/physics/src/push_solve.rs
Normal file
349
engine/physics/src/push_solve.rs
Normal file
@ -0,0 +1,349 @@
|
||||
use strafesnet_common::integer::{self,vec3::{self,Vector3},Fixed,Planar64,Planar64Vec3,Ratio};
|
||||
|
||||
// This algorithm is based on Lua code
|
||||
// written by Trey Reynolds in 2021
|
||||
|
||||
// EPSILON=1/2^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>;
|
||||
|
||||
// hack to allow comparing ratios to zero
|
||||
const RATIO_ZERO:Ratio<Fixed<1,32>,Fixed<1,32>>=Ratio::new(Fixed::ZERO,Fixed::EPSILON);
|
||||
|
||||
struct Ray{
|
||||
origin:Planar64Vec3,
|
||||
direction:Planar64Vec3,
|
||||
}
|
||||
impl Ray{
|
||||
fn extrapolate<Num,Den,N1,T1>(&self,t:Ratio<Num,Den>)->Planar64Vec3
|
||||
where
|
||||
Num:Copy,
|
||||
Den:Copy,
|
||||
Num:core::ops::Mul<Planar64,Output=N1>,
|
||||
Planar64:core::ops::Mul<Den,Output=N1>,
|
||||
N1:integer::Divide<Den,Output=T1>,
|
||||
T1:integer::Fix<Planar64>,
|
||||
{
|
||||
self.origin+self.direction.map(|elem|(t*elem).divide().fix())
|
||||
}
|
||||
}
|
||||
|
||||
/// 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)->Option<Ratio<Vector3<Fixed<3,96>>,Fixed<2,64>>>{
|
||||
const EPSILON:Fixed<2,64>=Fixed::from_bits(Fixed::<2,64>::ONE.to_bits().shr(10));
|
||||
let det=c0.normal.dot(c0.velocity);
|
||||
if det.abs()<EPSILON{
|
||||
return None;
|
||||
}
|
||||
let d0=c0.normal.dot(c0.position);
|
||||
Some(c0.normal*d0/det)
|
||||
}
|
||||
fn solve2(c0:&Contact,c1:&Contact)->Option<Ratio<Vector3<Fixed<5,160>>,Fixed<4,128>>>{
|
||||
const EPSILON:Fixed<4,128>=Fixed::from_bits(Fixed::<4,128>::ONE.to_bits().shr(10));
|
||||
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.abs()<EPSILON{
|
||||
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<Ratio<Vector3<Fixed<4,128>>,Fixed<3,96>>>{
|
||||
const EPSILON:Fixed<3,96>=Fixed::from_bits(Fixed::<3,96>::ONE.to_bits().shr(10));
|
||||
let n0_n1=c0.normal.cross(c1.normal);
|
||||
let det=c2.normal.dot(n0_n1);
|
||||
if det.abs()<EPSILON{
|
||||
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<[Ratio<Fixed<2,64>,Fixed<2,64>>;1]>{
|
||||
let det=u0.dot(u0);
|
||||
if det==Fixed::ZERO{
|
||||
return None;
|
||||
}
|
||||
let s0=u0.dot(point)/det;
|
||||
Some([s0])
|
||||
}
|
||||
fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->Option<[Ratio<Fixed<4,128>,Fixed<4,128>>;2]>{
|
||||
let u0_u1=u0.cross(u1);
|
||||
let det=u0_u1.dot(u0_u1);
|
||||
if det==Fixed::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<[Ratio<Fixed<3,96>,Fixed<3,96>>;3]>{
|
||||
let det=u0.cross(u1).dot(u2);
|
||||
if det==Fixed::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)==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)->Ray{
|
||||
Ray{origin:point,direction:vec3::ZERO}
|
||||
}
|
||||
fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option<Ray>{
|
||||
let direction=solve1(c0)?.divide().fix_1();
|
||||
let [s0]=decompose1(direction,c0.velocity)?;
|
||||
if s0.lt_ratio(RATIO_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)?.divide().fix_1();
|
||||
let [s0,s1]=decompose2(direction,c0.velocity,c1.velocity)?;
|
||||
if s0.lt_ratio(RATIO_ZERO)||s1.lt_ratio(RATIO_ZERO){
|
||||
return None;
|
||||
}
|
||||
let origin=point+solve2(
|
||||
&c0.relative_to(point),
|
||||
&c1.relative_to(point),
|
||||
)?.divide().fix_1();
|
||||
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)?.divide().fix_1();
|
||||
let [s0,s1,s2]=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?;
|
||||
if s0.lt_ratio(RATIO_ZERO)||s1.lt_ratio(RATIO_ZERO)||s2.lt_ratio(RATIO_ZERO){
|
||||
return None;
|
||||
}
|
||||
let origin=point+solve3(
|
||||
&c0.relative_to(point),
|
||||
&c1.relative_to(point),
|
||||
&c2.relative_to(point),
|
||||
)?.divide().fix_1();
|
||||
Some(Ray{origin,direction})
|
||||
}
|
||||
|
||||
const fn get_best_push_ray_and_conts_0<'a>(point:Planar64Vec3)->(Ray,Conts<'a>){
|
||||
(get_push_ray_0(point),Conts::new_const())
|
||||
}
|
||||
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:&[&'a Contact],
|
||||
)->Option<(Ray,Conts<'a>)>{
|
||||
match conts{
|
||||
&[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),
|
||||
&[]=>Some(get_best_push_ray_and_conts_0(point)),
|
||||
_=>unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
fn get_first_touch<'a>(contacts:&'a [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:&[Contact],point:Planar64Vec3)->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,cont))=>(t,cont),
|
||||
None=>return ray.origin,
|
||||
};
|
||||
|
||||
if RATIO_ZERO.le_ratio(next_t){
|
||||
return 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.as_slice()){
|
||||
Some((new_ray,new_conts))=>(ray,conts)=(new_ray,new_conts),
|
||||
None=>return 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!(
|
||||
vec3::ZERO,
|
||||
push_solve(&contacts,vec3::NEG_Y)
|
||||
);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user