diff --git a/src/push_solve.rs b/src/push_solve.rs
index aa61ba9..0574f16 100644
--- a/src/push_solve.rs
+++ b/src/push_solve.rs
@@ -1,4 +1,4 @@
-use strafesnet_common::integer::{Planar64,Planar64Vec3};
+use strafesnet_common::integer::{vec3::{self, Vector3}, Fixed, Planar64, Planar64Vec3, Ratio};
// This algorithm is based on Lua code
// written by Trey Reynolds in 2021
@@ -34,96 +34,78 @@ impl Contact{
normal:self.normal,
}
}
- fn relative_dot(&self,direction:Planar64Vec3)->Planar64{
+ 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)->Planar64{
+ 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<Planar64Vec3>{
+fn solve1(c0:&Contact)->Ratio<Vector3<Fixed<3,96>>,Fixed<2,64>>{
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)
+ c0.normal*d0/det
}
-fn solve2(c0:&Contact,c1:&Contact)->Option<Planar64Vec3>{
+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);
- 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)
+ (c1.normal.cross(u0_u1)*d0+u0_u1.cross(c0.normal)*d1)/det
}
-fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Option<Planar64Vec3>{
+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);
- 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)
+ (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>{
+fn decompose1(point:Planar64Vec3,u0:Planar64Vec3)->[Ratio<Fixed<2,64>,Fixed<2,64>>;1]{
let det=u0.dot(u0);
- if det==Planar64::ZERO{
- return None;
- }
let s0=u0.dot(point)/det;
- Some(s0)
+ [s0]
}
-fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->Option<(Planar64,Planar64)>{
+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);
- 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))
+ [s0,s1]
}
-fn decompose3(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3,u2:Planar64Vec3)->Option<(Planar64,Planar64,Planar64)>{
+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);
- 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))
+ [s0,s1,s2]
}
fn is_space_enclosed_2(
a:Planar64Vec3,
b:Planar64Vec3,
)->bool{
- a.cross(b)==Planar64Vec3::ZERO
- &&a.dot(b)<Planar64::ZERO
+ 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)==Planar64::ZERO
+ 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<=Planar64::ZERO
- && det_abbc*det_acbc<=Planar64::ZERO
- &&-det_acbc*det_abac<=Planar64::ZERO
+ 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)
@@ -139,12 +121,12 @@ fn is_space_enclosed_4(
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
+ 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)
@@ -152,17 +134,17 @@ fn is_space_enclosed_4(
}
const fn get_push_ray_0(point:Planar64Vec3)->Option<Ray>{
- Some(Ray{origin:point,direction:Planar64Vec3::ZERO})
+ 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)?;
+ 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>{
@@ -209,7 +191,7 @@ fn get_best_push_ray_and_conts_2<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Co
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){
+ if !c1.relative_dot(ray.direction).is_negative(){
return Some((ray,Conts::from_iter([c0])));
}
}
@@ -223,18 +205,18 @@ fn get_best_push_ray_and_conts_3<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Co
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){
+ 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 Planar64::ZERO<=c1.relative_dot(ray.direction){
+ 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 Planar64::ZERO<=c1.relative_dot(ray.direction)
- &&Planar64::ZERO<=c2.relative_dot(ray.direction){
+ if !c1.relative_dot(ray.direction).is_negative()
+ &&!c2.relative_dot(ray.direction).is_negative(){
return Some((ray,Conts::from_iter([c0])));
}
}
@@ -279,11 +261,11 @@ fn get_best_push_ray_and_conts<'a>(
}
}
-fn get_first_touch<'a>(contacts:&'a Vec<Contact>,ray:&Ray,conts:&Conts)->Option<(Planar64,&'a Contact)>{
+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)< -EPSILON
+ &&contact.relative_dot(ray.direction).is_negative()
)
.map(|contact|(contact.solve(ray),contact))
.min_by_key(|&(t,_)|t)
@@ -326,14 +308,14 @@ mod tests{
fn test_push_solve(){
let contacts=vec![
Contact{
- position:Planar64Vec3::ZERO,
- velocity:Planar64Vec3::Y,
- normal:Planar64Vec3::Y,
+ position:vec3::ZERO,
+ velocity:vec3::Y,
+ normal:vec3::Y,
}
];
assert_eq!(
- Some(Planar64Vec3::ZERO),
- push_solve(&contacts,Planar64Vec3::NEG_Y)
+ Some(vec3::ZERO),
+ push_solve(&contacts,vec3::NEG_Y)
);
}
}