diff --git a/src/push_solve.rs b/src/push_solve.rs index b2d32cc..5db32f0 100644 --- a/src/push_solve.rs +++ b/src/push_solve.rs @@ -46,24 +46,70 @@ impl CI<'_>{ } } -const fn solve0()->Planar64Vec3{ - Planar64Vec3::ZERO +//note that this is horrible with fixed point arithmetic +fn solve1(c0:&Contact)->Option{ + let det=c0.normal.dot(c0.velocity); + if det.get().abs()Option{ + 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()Option{ + let n0_n1=c0.normal.cross(c1.normal); + let det=c2.normal.dot(n0_n1); + if det.get().abs()Option{ - let d0=c0.normal.dot(c0.position); - let det=c0.normal.dot(c0.velocity); - if det.abs()Option{ + let det=u0.dot(u0); + if det==Planar64::ZERO{ + return None; } + let s0=u0.dot(point)/det; + Some(s0) +} +fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->Option<(Planar64,Planar64)>{ + 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)) +} +fn decompose3(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3,u2:Planar64Vec3)->Option<(Planar64,Planar64,Planar64)>{ + 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)) } const fn is_space_enclosed_0_1()->bool{ false } - fn is_space_enclosed_2( a:Planar64Vec3, b:Planar64Vec3,