551 lines
14 KiB
Rust
551 lines
14 KiB
Rust
use strafesnet_common::integer::vec3;
|
|
use strafesnet_common::integer::vec3::Vector3;
|
|
use strafesnet_common::integer::{Fixed,Planar64,Planar64Vec3};
|
|
|
|
use crate::model::{MeshQuery,MinkowskiMesh,MinkowskiVert,SubmeshVertId};
|
|
|
|
// This algorithm is based on Lua code
|
|
// written by Trey Reynolds in 2021
|
|
|
|
const SIMPLEX_TETRAHEDRON:usize=4;
|
|
// TODO: consider using an enum?
|
|
// enum Simplex{
|
|
// Simplex1([Vert;1]),
|
|
// Simplex2([Vert;2]),
|
|
// }
|
|
type Simplex=arrayvec::ArrayVec<MinkowskiVert,SIMPLEX_TETRAHEDRON>;
|
|
|
|
/*
|
|
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
|
|
*/
|
|
fn simplex_abs_det_is_zero(mesh:&MinkowskiMesh,simplex:&Simplex)->bool{
|
|
match simplex.as_slice(){
|
|
&[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
|
|
},
|
|
&[p0,p1,p2]=>{
|
|
let p0=mesh.vert(p0);
|
|
let p1=mesh.vert(p1);
|
|
let p2=mesh.vert(p2);
|
|
(p1-p0).cross(p2-p0)==const{Vector3::new([Fixed::ZERO,Fixed::ZERO,Fixed::ZERO])}
|
|
},
|
|
&[p0,p1]=>{
|
|
let p0=mesh.vert(p0);
|
|
let p1=mesh.vert(p1);
|
|
p1-p0==vec3::zero()
|
|
},
|
|
&[_p0]=>false,// 1 == 0
|
|
&[]=>false,
|
|
_=>unreachable!(),
|
|
}
|
|
}
|
|
|
|
/*
|
|
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();
|
|
match x.min(y).min(z){
|
|
min if min==x=>Vector3::new([y*y+z*z,-x*y,-x*z]).wrap_1(),
|
|
min if min==y=>Vector3::new([-x*y,x*x+z*z,-y*z]).wrap_1(),
|
|
min if min==z=>Vector3::new([-x*z,-y*z,x*x+y*y]).wrap_1(),
|
|
_=>unreachable!(),
|
|
}
|
|
}
|
|
|
|
const fn choose_any_direction()->Planar64Vec3{
|
|
vec3::X
|
|
}
|
|
|
|
fn reduce_simplex(
|
|
mesh:&MinkowskiMesh,
|
|
mut simplex:Simplex,
|
|
)->(Planar64Vec3,Simplex){
|
|
match simplex.len(){
|
|
0=>(choose_any_direction(),simplex),
|
|
// local function reduceSimplex0(a0, a1)
|
|
1=>{
|
|
// --debug.profilebegin("reduceSimplex0")
|
|
// local a = a1 - a0
|
|
let p0=mesh.vert(simplex[0]);
|
|
|
|
// local p = -a
|
|
let p=-p0;
|
|
|
|
// local direction = p
|
|
let mut direction=p;
|
|
|
|
// if direction.magnitude == 0 then
|
|
// direction = chooseAnyDirection()
|
|
if direction==vec3::zero(){
|
|
direction=choose_any_direction();
|
|
}
|
|
|
|
// return direction, a0, a1
|
|
(direction,simplex)
|
|
},
|
|
// local function reduceSimplex1(a0, a1, b0, b1)
|
|
2=>{
|
|
// --debug.profilebegin("reduceSimplex1")
|
|
// local a = a1 - a0
|
|
// local b = b1 - b0
|
|
let p0=mesh.vert(simplex[0]);
|
|
let p1=mesh.vert(simplex[1]);
|
|
|
|
// local p = -a
|
|
// local u = b - a
|
|
let p=-p0;
|
|
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==const{Vector3::new([Fixed::ZERO,Fixed::ZERO,Fixed::ZERO])}{
|
|
return (choose_perpendicular_direction(u),simplex);
|
|
}
|
|
|
|
// -- modify the direction to take into account a0R and b0R
|
|
// return direction, a0, a1, b0, b1
|
|
return (direction.narrow_1().unwrap(),simplex)
|
|
}
|
|
|
|
simplex.remove(1);
|
|
|
|
// local direction = p
|
|
let mut direction=p;
|
|
|
|
// if direction.magnitude == 0 then
|
|
if direction==vec3::zero(){
|
|
direction=choose_perpendicular_direction(u);
|
|
}
|
|
|
|
// return direction, a0, a1
|
|
(direction,simplex)
|
|
},
|
|
// local function reduceSimplex2(a0, a1, b0, b1, c0, c1)
|
|
3=>{
|
|
// --debug.profilebegin("reduceSimplex2")
|
|
// local a = a1 - a0
|
|
// local b = b1 - b0
|
|
// local c = c1 - c0
|
|
let p0=mesh.vert(simplex[0]);
|
|
let p1=mesh.vert(simplex[1]);
|
|
let p2=mesh.vert(simplex[2]);
|
|
|
|
// local p = -a
|
|
// local u = b - a
|
|
// local v = c - a
|
|
let p=-p0;
|
|
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 (direction.narrow_1().unwrap(),simplex)
|
|
}
|
|
|
|
// 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_u=u.dot(u);
|
|
let v_v=v.dot(v);
|
|
let u_dist=uv_up*(v_v*u.length());
|
|
let v_dist=uv_pv*(u_u*v.length());
|
|
|
|
// if vDist == minDist2 then
|
|
if v_dist<u_dist{
|
|
u=v;
|
|
up=-pv;
|
|
uv=-uv;
|
|
// u_u=v_v; // unused
|
|
// b0 = c0
|
|
// b1 = c1
|
|
simplex.remove(1);
|
|
}else{
|
|
simplex.remove(2);
|
|
}
|
|
|
|
// 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 (uv.narrow_1().unwrap(),simplex)
|
|
}
|
|
|
|
// return direction, a0, a1, b0, b1
|
|
return (direction.narrow_1().unwrap(),simplex)
|
|
}
|
|
|
|
simplex.remove(1);
|
|
|
|
// local direction = p
|
|
let direction=p;
|
|
// if direction.magnitude == 0 then
|
|
if direction==vec3::zero(){
|
|
// direction = uv
|
|
return (uv.narrow_1().unwrap(),simplex)
|
|
}
|
|
// return direction, a0, a0
|
|
(direction,simplex)
|
|
},
|
|
// local function reduceSimplex3(a0, a1, b0, b1, c0, c1, d0, d1)
|
|
4=>{
|
|
// --debug.profilebegin("reduceSimplex3")
|
|
// local a = a1 - a0
|
|
// local b = b1 - b0
|
|
// local c = c1 - c0
|
|
// local d = d1 - d0
|
|
let p0=mesh.vert(simplex[0]);
|
|
let p1=mesh.vert(simplex[1]);
|
|
let p2=mesh.vert(simplex[2]);
|
|
let p3=mesh.vert(simplex[3]);
|
|
|
|
// local p = -a
|
|
// local u = b - a
|
|
// local v = c - a
|
|
// local w = d - a
|
|
let p=-p0;
|
|
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 (vec3::zero(),simplex);
|
|
}
|
|
|
|
// 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)*vw.length()*wu.length();
|
|
let vw_dist=pv_w.mul_sign(uv_w)*wu.length()*uv.length();
|
|
let wu_dist=up_w.mul_sign(uv_w)*uv.length()*vw.length();
|
|
let min_dist=uv_dist.min(vw_dist).min(wu_dist);
|
|
|
|
// if vwDist == minDist3 then
|
|
if vw_dist==min_dist{
|
|
(u,v)=(v,w);
|
|
uv=vw;
|
|
// uv_p=pv_w; // unused
|
|
// b0, c0 = c0, d0
|
|
// b1, c1 = c1, d1
|
|
simplex.remove(1);
|
|
// elseif wuDist == minDist3 then
|
|
}else if wu_dist==min_dist{
|
|
(u,v)=(w,u);
|
|
uv=wu;
|
|
// uv_p=up_w; // unused
|
|
// b0, c0 = d0, b0
|
|
// b1, c1 = d1, b1
|
|
// before [a,b,c,d]
|
|
simplex[2]=simplex[1];
|
|
simplex.swap_remove(1);
|
|
// after [a,d,b]
|
|
}else{
|
|
simplex.remove(2);
|
|
}
|
|
|
|
// 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
|
|
if uv_w.is_negative(){
|
|
return (uv.narrow_1().unwrap(),simplex);
|
|
}else{
|
|
return (-uv.narrow_1().unwrap(),simplex);
|
|
}
|
|
}
|
|
|
|
// 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_u=u.dot(u);
|
|
let v_v=v.dot(v);
|
|
let u_dist=uv_up*(v_v*u.length());
|
|
let v_dist=uv_pv*(u_u*v.length());
|
|
|
|
// if vDist == minDist2 then
|
|
if v_dist<u_dist{
|
|
u=v;
|
|
up=-pv;
|
|
uv=-uv;
|
|
// u_u=v_v; // unused
|
|
// b0 = c0
|
|
// b1 = c1
|
|
simplex.remove(1);
|
|
}else{
|
|
simplex.remove(2);
|
|
}
|
|
|
|
// 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
|
|
if uv_w.is_negative(){
|
|
return (uv.narrow_1().unwrap(),simplex);
|
|
}else{
|
|
return (-uv.narrow_1().unwrap(),simplex);
|
|
}
|
|
}
|
|
|
|
// return direction, a0, a1, b0, b1
|
|
return (direction.narrow_1().unwrap(),simplex)
|
|
}
|
|
|
|
simplex.remove(1);
|
|
|
|
// local direction = p
|
|
let direction=p;
|
|
// if direction.magnitude == 0 then
|
|
if direction==vec3::zero(){
|
|
// direction = uvw < 0 and uv or -uv
|
|
if uv_w.is_negative(){
|
|
return (uv.narrow_1().unwrap(),simplex);
|
|
}else{
|
|
return (-uv.narrow_1().unwrap(),simplex);
|
|
}
|
|
}
|
|
|
|
// return direction, a0, a1
|
|
(direction,simplex)
|
|
},
|
|
_=>unreachable!(),
|
|
}
|
|
}
|
|
|
|
/// Intermediate data structure containing a partially complete calculation.
|
|
/// Sometimes you only care about the topology, and not about the
|
|
/// exact point of intersection details.
|
|
pub struct Topology{
|
|
simplex:Simplex,
|
|
}
|
|
impl Topology{
|
|
pub fn details(self,mesh:&MinkowskiMesh)->Details{
|
|
unimplemented!()
|
|
}
|
|
}
|
|
pub struct Details{
|
|
// distance:Planar64,
|
|
// p_pos:Planar64Vec3,
|
|
// p_norm:Planar64Vec3,
|
|
// q_pos:Planar64Vec3,
|
|
// q_norm:Planar64Vec3,
|
|
}
|
|
|
|
pub fn contains_point(mesh:&MinkowskiMesh,point:Planar64Vec3)->bool{
|
|
const ENABLE_FAST_FAIL:bool=true;
|
|
minimum_difference::<ENABLE_FAST_FAIL,_>(mesh,point,
|
|
// expanded
|
|
|_simplex|{
|
|
// local norm = direction.unit
|
|
// local dist = a:Dot(norm)
|
|
// local hits = -dist < radiusP + radiusQ
|
|
// return hits
|
|
unimplemented!()
|
|
},
|
|
// unexpanded
|
|
|_simplex|{
|
|
// intersection is guaranteed at this point
|
|
true
|
|
},
|
|
// fast_fail value
|
|
false
|
|
)
|
|
}
|
|
pub fn closest_fev(mesh:&MinkowskiMesh,point:Planar64Vec3)->Topology{
|
|
const ENABLE_FAST_FAIL:bool=false;
|
|
minimum_difference::<ENABLE_FAST_FAIL,_>(mesh,point,
|
|
// expanded
|
|
|_simplex|{
|
|
// NOTE: if hits is true, this if statement necessarily evaluates to true.
|
|
// i.e. hits implies this statement
|
|
// if -dist <= exitRadius + radiusP + radiusQ then
|
|
// local posP, posQ = decompose(-rel_pos, a0, a1, b0, b1, c0, c1)
|
|
// return hits, -dist - radiusP - radiusQ,
|
|
// posP - radiusP*norm, -norm,
|
|
// posQ + radiusQ*norm, norm
|
|
// end
|
|
// return false
|
|
unimplemented!()
|
|
},
|
|
// unexpanded
|
|
|_simplex|{
|
|
// local norm, dist, u0, u1, v0, v1, w0, w1 = expand(queryP, queryQ, a0, a1, b0, b1, c0, c1, d0, d1, 1e-5)
|
|
// if norm then
|
|
// local posP, posQ = decompose(-rel_pos, u0, u1, v0, v1, w0, w1)
|
|
// return true, -dist - radiusP - radiusQ,
|
|
// posP - radiusP*norm, -norm,
|
|
// posQ + radiusQ*norm, norm
|
|
// end
|
|
// return nil
|
|
unimplemented!()
|
|
},
|
|
// fast_fail value is irrelevant and will never be returned!
|
|
Topology{simplex:Simplex::new_const()}
|
|
)
|
|
}
|
|
|
|
// local function minimumDifference(
|
|
// queryP, radiusP,
|
|
// queryQ, radiusQ,
|
|
// exitRadius, testIntersection
|
|
// )
|
|
fn minimum_difference<const ENABLE_FAST_FAIL:bool,T>(
|
|
mesh:&MinkowskiMesh,
|
|
rel_pos:Planar64Vec3,
|
|
expanded:impl Fn(Simplex)->T,
|
|
unexpanded:impl Fn(Simplex)->T,
|
|
fast_fail:T,
|
|
)->T{
|
|
// 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 direction=mesh.hint_point()+rel_pos;
|
|
let new_point=mesh.farthest_vert(direction);
|
|
let mut simplex=Simplex::from_iter([new_point]);
|
|
|
|
// exitRadius = testIntersection and 0 or exitRadius or 1/0
|
|
// for _ = 1, 100 do
|
|
loop{
|
|
// if a and b and c and d then
|
|
if simplex.len()==SIMPLEX_TETRAHEDRON{
|
|
return unexpanded(simplex);
|
|
}
|
|
|
|
// 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);
|
|
|
|
// if -direction:Dot(next_point) > (exitRadius + radiusP + radiusQ)*direction.magnitude then
|
|
if ENABLE_FAST_FAIL&&direction.dot(mesh.vert(next_point)+rel_pos).is_negative(){
|
|
return fast_fail;
|
|
}
|
|
|
|
// push_front
|
|
if simplex.len()==simplex.capacity(){
|
|
// this is a dead case, new_simplex never has more than 3 elements
|
|
simplex.rotate_right(1);
|
|
simplex[0]=next_point;
|
|
}else{
|
|
simplex.push(next_point);
|
|
simplex.rotate_right(1);
|
|
}
|
|
|
|
// if
|
|
// direction:Dot(next_point - a) <= 0 or
|
|
// absDet(next_point, a, b, c) < 1e-6
|
|
if !direction.dot(mesh.vert(simplex[0])-mesh.vert(simplex[1])).is_positive()
|
|
||simplex_abs_det_is_zero(mesh,&simplex){
|
|
return expanded(simplex);
|
|
}
|
|
|
|
// direction, a0, a1, b0, b1, c0, c1, d0, d1 = reduceSimplex(new_point_p, new_point_q, a0, a1, b0, b1, c0, c1)
|
|
(direction,simplex)=reduce_simplex(mesh,simplex);
|
|
}
|
|
}
|