diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs index 51c664d2..c951debd 100644 --- a/engine/physics/src/minimum_difference.rs +++ b/engine/physics/src/minimum_difference.rs @@ -130,7 +130,7 @@ fn reduce_simplex( // 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); + let direction=u.cross(p).cross(u); // if direction.magnitude == 0 then if direction==const{Vector3::new([Fixed::ZERO,Fixed::ZERO,Fixed::ZERO])}{ @@ -204,7 +204,7 @@ fn reduce_simplex( // 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 mut 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()); @@ -333,43 +333,83 @@ fn reduce_simplex( // 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 - // local direction = uvw < 0 and uv or -uv - // return direction, a0, a1, b0, b1, c0, c1 - // end + 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 mut 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 - // u = v - // b0 = c0 - // b1 = c1 - // up = -pv - // uv = -uv - // u_u = v_v - // end + if v_dist= 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 + 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.pop(); // local direction = p + let direction=p; // if direction.magnitude == 0 then - // direction = uvw < 0 and uv or -uv - // end + 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!(), }