naive solve and decompose

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<Planar64Vec3>{
+	let det=c0.normal.dot(c0.velocity);
+	if det.get().abs()<EPSILON.get(){
+		return None;
+	}
+	let d0=c0.normal.dot(c0.position);
+	Some(c0.normal*d0/det)
+}
+fn solve2(c0:&Contact,c1:&Contact)->Option<Planar64Vec3>{
+	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()<EPSILON.get(){
+		return None;
+	}
+	let d0=c0.normal.dot(c0.position);
+	let d1=c1.normal.dot(c1.position);
+	Some((c1.normal.cross(u0_u1)*d0+u0_u1.cross(c0.normal)*d1)/det)
+}
+fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Option<Planar64Vec3>{
+	let n0_n1=c0.normal.cross(c1.normal);
+	let det=c2.normal.dot(n0_n1);
+	if det.get().abs()<EPSILON.get(){
+		return None;
+	}
+	let d0=c0.normal.dot(c0.position);
+	let d1=c1.normal.dot(c1.position);
+	let d2=c2.normal.dot(c2.position);
+	Some((c1.normal.cross(c2.normal)*d0+c2.normal.cross(c0.normal)*d1+c0.normal.cross(c1.normal)*d2)/det)
 }
 
-fn solve1(c0:&Contact)->Option<Planar64Vec3>{
-	let d0=c0.normal.dot(c0.position);
-	let det=c0.normal.dot(c0.velocity);
-	if det.abs()<EPSILON{
-		None
-	}else{
-		Some(c0.normal*d0/det)
+fn decompose1(point:Planar64Vec3,u0:Planar64Vec3)->Option<Planar64>{
+	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,