work

engine/physics/src/minimum_difference.rs

@@ -2,7 +2,7 @@ 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};
+use crate::model::{MeshQuery,MinkowskiMesh,MinkowskiVert,SubmeshVertId};
 
 // This algorithm is based on Lua code
 // written by Trey Reynolds in 2021
@@ -63,56 +63,103 @@ local function choosePerpendicularDirection(d)
 	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!(),
+	}
+}
 
-/*
-local function chooseAnyDirection()
-	return Vector3.new(1, 0, 0)
-end
-*/
+const fn choose_any_direction()->Planar64Vec3{
+	vec3::X
+}
 
-/*
-local function reduceSimplex0(a0, a1)
-	--debug.profilebegin("reduceSimplex0")
-	local a = a1 - a0
+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
+			// local p = -a
+			let p=-p0;
 
-	local direction = p
-	if direction.magnitude == 0 then
-		direction = chooseAnyDirection()
-	end
-	return direction, a0, a1
-end
-*/
+			// local direction = p
+			let mut direction=p;
 
-/*
-local function reduceSimplex1(a0, a1, b0, b1)
-	--debug.profilebegin("reduceSimplex1")
-	local a = a1 - a0
-	local b = b1 - b0
+			// if direction.magnitude == 0 then
+			// 	direction = chooseAnyDirection()
+			if direction==vec3::ZERO{
+				direction=choose_any_direction();
+			}
 
-	local p = -a
-	local u = b - a
+			// 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]);
 
-	-- modify to take into account the radiuses
-	local p_u = p:Dot(u)
+			// local p = -a
+			// local u = b - a
+			let p=-p0;
+			let u=p1-p0;
 
-	if p_u >= 0 then
-		local direction = u:Cross(p):Cross(u)
-		if direction.magnitude == 0 then
-			direction = choosePerpendicularDirection(u)
-		end
-		-- modify the direction to take into account a0R and b0R
-		return direction, a0, a1, b0, b1
-	end
+			// -- modify to take into account the radiuses
+			// local p_u = p:Dot(u)
+			let p_u=p.dot(u);
 
-	local direction = p
-	if direction.magnitude == 0 then
-		direction = choosePerpendicularDirection(u)
-	end
-	return direction, a0, a1
-end
-*/
+			// 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==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)
+		},
+		3=>{
+			(_,_)
+		},
+		4=>{
+			(_,_)
+		},
+		_=>unreachable!(),
+	}
+}
 
 /*
 local function reduceSimplex2(a0, a1, b0, b1, c0, c1)
@@ -267,19 +314,6 @@ local function reduceSimplex3(a0, a1, b0, b1, c0, c1, d0, d1)
 end
 */
 
-fn reduce_simplex(
-	simplex:Simplex,
-)->(Planar64Vec3,Simplex){
-	match simplex.as_slice(){
-		&[p0,p1,p2,p3]=>reduce_simplex_4(p0,p1,p2,p3),
-		&[p0,p1,p2]=>reduce_simplex_3(p0,p1,p2),
-		&[p0,p1]=>reduce_simplex_2(p0,p1),
-		&[p0]=>reduce_simplex_1(p0),
-		&[]=>reduce_simplex_0(),
-		_=>unreachable!(),
-	}
-}
-
 enum Expanded{
 	Expanded,
 	Unexpanded,
@@ -386,6 +420,6 @@ pub fn minimum_difference(mesh:&MinkowskiMesh,rel_pos:Planar64Vec3)->Option<Mini
 		}
 
 		// 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(simplex);
+		(direction,simplex)=reduce_simplex(mesh,simplex);
 	}
 }