472 lines
12 KiB
Rust
472 lines
12 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 mut 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.pop();
|
|
|
|
// 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;
|
|
// b0 = c0
|
|
// b1 = c1
|
|
simplex[1]=simplex[2];
|
|
}
|
|
|
|
simplex.pop();
|
|
|
|
// 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.pop();
|
|
|
|
// 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 u=p1-p0;
|
|
let 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 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
|
|
// local direction = Vector3.new(0, 0, 0)
|
|
// return direction, a0, a1, b0, b1, c0, c1, d0, d1
|
|
// end
|
|
|
|
// 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)
|
|
|
|
// if vwDist == minDist3 then
|
|
// u, v = v, w
|
|
// b0, c0 = c0, d0
|
|
// b1, c1 = c1, d1
|
|
// uv = vw
|
|
// uvp = pvw
|
|
// elseif wuDist == minDist3 then
|
|
// u, v = w, u
|
|
// b0, c0 = d0, b0
|
|
// b1, c1 = d1, b1
|
|
// uv = wu
|
|
// uvp = upw
|
|
// end
|
|
|
|
// local up = u:Cross(p)
|
|
// local pv = p:Cross(v)
|
|
// local uv_up = uv:Dot(up)
|
|
// local uv_pv = uv:Dot(pv)
|
|
|
|
// if uv_up >= 0 and uv_pv >= 0 then
|
|
// local direction = uvw < 0 and uv or -uv
|
|
// return direction, a0, a1, b0, b1, c0, c1
|
|
// end
|
|
|
|
// 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)
|
|
|
|
// if vDist == minDist2 then
|
|
// u = v
|
|
// b0 = c0
|
|
// b1 = c1
|
|
// up = -pv
|
|
// uv = -uv
|
|
// u_u = v_v
|
|
// end
|
|
|
|
// local p_u = p:Dot(u)
|
|
|
|
// if p_u >= 0 then
|
|
// local direction = up:Cross(u)
|
|
// if direction.magnitude == 0 then
|
|
// direction = uvw < 0 and uv or -uv
|
|
// end
|
|
// return direction, a0, a1, b0, b1
|
|
// end
|
|
|
|
// local direction = p
|
|
// if direction.magnitude == 0 then
|
|
// direction = uvw < 0 and uv or -uv
|
|
// end
|
|
// return direction, a0, a1
|
|
(_,_)
|
|
},
|
|
_=>unreachable!(),
|
|
}
|
|
}
|
|
|
|
enum Expanded{
|
|
Expanded,
|
|
Unexpanded,
|
|
}
|
|
|
|
/// Intermediate data structure containing a partially complete calculation.
|
|
/// Sometimes you only care if the meshes are intersecting, and not about the
|
|
/// exact point of intersection details.
|
|
pub struct MinimumDifference{
|
|
expanded:Expanded,
|
|
}
|
|
impl MinimumDifference{
|
|
pub fn details(&self,mesh:&MinkowskiMesh,rel_pos:Planar64Vec3)->Details{
|
|
match self.expanded{
|
|
Expanded::Expanded=>{
|
|
// local norm = direction.unit
|
|
// local dist = a:Dot(norm)
|
|
// local hits = -dist < radiusP + radiusQ
|
|
// if testIntersection then
|
|
// return hits
|
|
// end
|
|
// 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
|
|
Details{}
|
|
},
|
|
Expanded::Unexpanded=>{
|
|
// if testIntersection then
|
|
// return true
|
|
// end
|
|
// 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
|
|
Details{}
|
|
},
|
|
}
|
|
}
|
|
}
|
|
pub struct Details{
|
|
distance:Planar64,
|
|
p_pos:Planar64Vec3,
|
|
p_norm:Planar64Vec3,
|
|
q_pos:Planar64Vec3,
|
|
q_norm:Planar64Vec3,
|
|
}
|
|
|
|
// local function minimumDifference(
|
|
// queryP, radiusP,
|
|
// queryQ, radiusQ,
|
|
// exitRadius, testIntersection
|
|
// )
|
|
pub fn minimum_difference(mesh:&MinkowskiMesh,rel_pos:Planar64Vec3)->Option<MinimumDifference>{
|
|
// 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 Some(MinimumDifference::unexpanded());
|
|
}
|
|
|
|
// 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 direction.dot(mesh.vert(next_point)+rel_pos).is_negative(){
|
|
return None;
|
|
}
|
|
|
|
// 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 Some(MinimumDifference::expanded());
|
|
}
|
|
|
|
// 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);
|
|
}
|
|
}
|