improved collision

src/Marble.hx

@@ -737,7 +737,7 @@ class Marble extends GameObject {
 		rPitch.value = pitch;
 	}
 
-	function testMove(velocity:Vector, position:Vector, deltaT:Float, radius:Float, testPIs:Bool):{position:Vector, t:Float, found:Bool} {
+	function testMove(velocity:Vector, position:Vector, deltaT:Float, radius:Float, testPIs:Bool) {
 		var searchbox = new Bounds();
 		searchbox.addSpherePos(position.x, position.y, position.z, _radius);
 		searchbox.addSpherePos(position.x + velocity.x * deltaT, position.y + velocity.y * deltaT, position.z + velocity.z * deltaT, _radius);
@@ -749,6 +749,12 @@ class Marble extends GameObject {
 
 		var lastContactPos = new Vector();
 
+		var testTriangles = [];
+
+		var finalContacts = [];
+
+		// for (iter in 0...10) {
+		//	var iterationFound = false;
 		for (obj in foundObjs.filter(x -> x.go is InteriorObject && !(x.go is PathedInterior))) {
 			// Its an MP so bruh
 
@@ -758,77 +764,96 @@ class Marble extends GameObject {
 			var localpos = position.clone();
 			localpos.transform(invMatrix);
 
-			var relLocalVel = velocity.sub(obj.velocity);
+			var relVel = velocity.sub(obj.velocity);
-			relLocalVel.transform3x3(invMatrix);
+			var relLocalVel = relVel.transformed3x3(invMatrix);
 
 			var boundThing = new Bounds();
-			boundThing.addSpherePos(localpos.x, localpos.y, localpos.z, radius * 1.1);
+			boundThing.addSpherePos(localpos.x, localpos.y, localpos.z, radius * 2);
-			boundThing.addSpherePos(localpos.x + relLocalVel.x * deltaT, localpos.y + relLocalVel.y * deltaT, localpos.z + relLocalVel.z * deltaT,
+			boundThing.addSpherePos(localpos.x
-				radius * 1.1);
+				+ relLocalVel.x * deltaT * 5, localpos.y
+				+ relLocalVel.y * deltaT * 5, localpos.z
+				+ relLocalVel.z * deltaT * 5,
+				radius * 2);
 
 			var surfaces = obj.bvh == null ? obj.octree.boundingSearch(boundThing).map(x -> cast x) : obj.bvh.boundingSearch(boundThing);
 
 			for (surf in surfaces) {
 				var surface:CollisionSurface = cast surf;
 
-				var currentFinalPos = localpos.add(relLocalVel.multiply(finalT));
+				var currentFinalPos = position.add(relVel.multiply(finalT)); // localpos.add(relLocalVel.multiply(finalT));
 
 				var i = 0;
 				while (i < surface.indices.length) {
-					var v0 = surface.points[surface.indices[i]];
+					var v0 = surface.points[surface.indices[i]].transformed(obj.transform);
-					var v = surface.points[surface.indices[i + 1]];
+					var v = surface.points[surface.indices[i + 1]].transformed(obj.transform);
-					var v2 = surface.points[surface.indices[i + 2]];
+					var v2 = surface.points[surface.indices[i + 2]].transformed(obj.transform);
 
-					var surfaceNormal = surface.normals[surface.indices[i]];
+					var triangleVerts = [v0, v, v2];
+
+					var surfaceNormal = surface.normals[surface.indices[i]].transformed3x3(obj.transform).normalized();
 					var surfaceD = -surfaceNormal.dot(v0);
 
 					// If we're going the wrong direction or not going to touch the plane, ignore...
-					if (surfaceNormal.dot(relLocalVel) > -0.001 || surfaceNormal.dot(currentFinalPos) + surfaceD > radius) {
+					if (surfaceNormal.dot(relVel) > -0.001 || surfaceNormal.dot(currentFinalPos) + surfaceD > radius) {
 						i += 3;
 						continue;
 					}
 
+					// var v0T = v0.transformed(obj.transform);
+					// var vT = v.transformed(obj.transform);
+					// var v2T = v2.transformed(obj.transform);
+					// var vN = surfaceNormal.transformed3x3(obj.transform);
+					testTriangles.push({
+						v: [v0, v, v2],
+						n: surfaceNormal,
+						edge: surf.edgeData[Math.floor(i / 3)],
+						concavity: surface.edgeConcavities.slice(Math.floor(i / 3), Math.floor(i / 3) + 3)
+					});
+
 					// Time until collision with the plane
-					var collisionTime = (-localpos.dot(surfaceNormal) - surfaceD + radius) / surfaceNormal.dot(relLocalVel);
+					var collisionTime = (radius - position.dot(surfaceNormal) - surfaceD) / surfaceNormal.dot(relVel);
 
 					// Are we going to touch the plane during this time step?
-					if (collisionTime > 0 && collisionTime < finalT) {
+					if (collisionTime >= 0.000001 && finalT >= collisionTime) {
-						var collisionPoint = localpos.add(relLocalVel.multiply(collisionTime));
+						var collisionPoint = position.add(relVel.multiply(collisionTime));
-						var lastPoint = v2;
+						// var lastPoint = v2;
-						var testPlane:h3d.col.Plane = null;
+						// var j = 0;
-						var j = 0;
+						// while (j < 3) {
-						while (j < 3) {
+						// 	var testPoint = surface.points[surface.indices[i + j]];
-							var testPoint = surface.points[surface.indices[i + j]];
+						// 	if (testPoint != lastPoint) {
-							if (testPoint != lastPoint) {
+						// 		var a = surfaceNormal;
-								testPlane = h3d.col.Plane.fromPoints(testPoint.add(surfaceNormal).toPoint(), testPoint.toPoint(), lastPoint.toPoint());
+						// 		var b = lastPoint.sub(testPoint);
-								lastPoint = testPoint;
+						// 		var planeNorm = b.cross(a);
-								// if we are on the far side of the edge
+						// 		var planeD = -planeNorm.dot(testPoint);
-								if (testPlane.distance(collisionPoint.toPoint()) < 0)
+						// 		lastPoint = testPoint;
-									break;
+						// 		// if we are on the far side of the edge
-							}
+						// 		if (planeNorm.dot(collisionPoint) + planeD >= 0.0)
-							j++;
+						// 			break;
-						}
+						// 	}
+						// 	j++;
+						// }
 						// If we're inside the poly, just get the position
-						if (j == 3) {
+						if (Collision.PointInTriangle(collisionPoint, v0, v, v2)) {
 							finalT = collisionTime;
-							currentFinalPos = localpos.add(relLocalVel.multiply(finalT));
+							currentFinalPos = position.add(relVel.multiply(finalT));
 							found = true;
+							// iterationFound = true;
+							i += 3;
 							continue;
 						}
 					}
-
 					// We *might* be colliding with an edge
 
 					var lastVert = v2;
 
 					var radSq = radius * radius;
-					for (iter in 0...surface.indices.length) {
+					for (iter in 0...3) {
-						var thisVert = surface.points[surface.indices[iter]];
+						var thisVert = triangleVerts[iter];
 
 						var vertDiff = lastVert.sub(thisVert);
-						var posDiff = localpos.sub(thisVert);
+						var posDiff = position.sub(thisVert);
 
-						var velRejection = vertDiff.cross(relLocalVel);
+						var velRejection = vertDiff.cross(relVel);
 						var posRejection = vertDiff.cross(posDiff);
 
 						// Build a quadratic equation to solve for the collision time
@@ -848,7 +873,7 @@ class Marble extends GameObject {
 						var discriminantSqrt = Math.sqrt(discriminant);
 
 						// Solve using the quadratic formula
-						var edgeCollisionTime = (discriminantSqrt - b) * oneOverTwoA;
+						var edgeCollisionTime = (-b + discriminantSqrt) * oneOverTwoA;
 						var edgeCollisionTime2 = (-b - discriminantSqrt) * oneOverTwoA;
 
 						// Make sure the 2 times are in ascending order
@@ -865,10 +890,18 @@ class Marble extends GameObject {
 						}
 
 						// Check if the collision hasn't already happened
-						if (edgeCollisionTime >= 0.00001) {
+						if (edgeCollisionTime >= 0.000001) {
+							// if (edgeCollisionTime < 0.000001) {
+							// 	edgeCollisionTime = edgeCollisionTime2;
+							// }
+							// if (edgeCollisionTime < 0.00001)
+							// 	continue;
+							// if (edgeCollisionTime > finalT)
+							// 	continue;
+
 							var edgeLen = vertDiff.length();
 
-							var relativeCollisionPos = localpos.add(relLocalVel.multiply(edgeCollisionTime)).sub(thisVert);
+							var relativeCollisionPos = position.add(relVel.multiply(edgeCollisionTime)).sub(thisVert);
 
 							var distanceAlongEdge = relativeCollisionPos.dot(vertDiff) / edgeLen;
 
@@ -881,21 +914,23 @@ class Marble extends GameObject {
 							// If the collision is within the edge, resolve the collision and continue.
 							if (distanceAlongEdge >= 0.0 && distanceAlongEdge <= edgeLen) {
 								finalT = edgeCollisionTime;
-								currentFinalPos = localpos.add(relLocalVel.multiply(finalT));
+								currentFinalPos = position.add(relVel.multiply(finalT));
+								lastContactPos = vertDiff.multiply(distanceAlongEdge / edgeLen).add(thisVert);
 								lastVert = thisVert;
 								found = true;
+								// iterationFound = true;
 								continue;
 							}
 						}
 
 						// This is what happens when we collide with a corner
 
-						a = relLocalVel.lengthSq();
+						a = relVel.lengthSq();
 
 						// Build a quadratic equation to solve for the collision time
-						var posVertDiff = localpos.sub(thisVert);
+						var posVertDiff = position.sub(thisVert);
-						b = 2 * posVertDiff.dot(relLocalVel);
+						b = 2 * posVertDiff.dot(relVel);
-						c = posVertDiff.length() - radSq;
+						c = posVertDiff.lengthSq() - radSq;
 						discriminant = b * b - (4 * a * c);
 
 						// If it's quadratic and has a solution ...
@@ -924,17 +959,18 @@ class Marble extends GameObject {
 								if (edgeCollisionTime >= 0.0) {
 									// Resolve it and continue
 									finalT = edgeCollisionTime;
-									currentFinalPos = localpos.add(relLocalVel.multiply(finalT));
+									currentFinalPos = position.add(relVel.multiply(finalT));
 									lastContactPos = thisVert;
 									found = true;
+									// iterationFound = true;
 								}
 							}
 						}
 
 						// We still need to check the other corner ...
 						// Build one last quadratic equation to solve for the collision time
-						posVertDiff = localpos.sub(lastVert);
+						posVertDiff = position.sub(lastVert);
-						b = 2 * posVertDiff.dot(relLocalVel);
+						b = 2 * posVertDiff.dot(relVel);
 						c = posVertDiff.lengthSq() - radSq;
 						discriminant = b * b - (4 * a * c);
 
@@ -958,28 +994,57 @@ class Marble extends GameObject {
 							edgeCollisionTime = temp;
 						}
 
-						if (edgeCollisionTime2 > 0.0001 && edgeCollisionTime < finalT) {
+						if (edgeCollisionTime2 <= 0.0001 || finalT <= edgeCollisionTime) {
-							if (edgeCollisionTime <= 0 && edgeCollisionTime > -0.0001) {
+							lastVert = thisVert;
-								edgeCollisionTime = 0;
+							continue;
-							}
-							if (edgeCollisionTime >= 0) {
-								finalT = edgeCollisionTime;
-								currentFinalPos = localpos.add(relLocalVel.multiply(finalT));
-								lastVert = thisVert;
-								found = true;
-							}
 						}
+
+						if (edgeCollisionTime <= 0.0 && edgeCollisionTime > -0.0001)
+							edgeCollisionTime = 0;
+
+						if (edgeCollisionTime < 0.000001) {
+							lastVert = thisVert;
+							continue;
+						}
+
+						finalT = edgeCollisionTime;
+						currentFinalPos = position.add(relVel.multiply(finalT));
+
+						lastVert = thisVert;
+						found = true;
+						// iterationFound = true;
 					}
 
 					i += 3;
 				}
 			}
 		}
+
+		//	if (!iterationFound)
+		//		break;
+		// }
 		var deltaPosition = velocity.multiply(finalT);
 		var finalPosition = position.add(deltaPosition);
 		position = finalPosition;
 
-		return {position: position, t: finalT, found: found};
+		// for (testTri in testTriangles) {
+		// 	var tsi = Collision.TriangleSphereIntersection(testTri.v[0], testTri.v[1], testTri.v[2], testTri.n, finalPosition, radius, testTri.edge,
+		// 		testTri.concavity);
+		// 	if (tsi.result) {
+		// 		var contact = new CollisionInfo();
+		// 		contact.point = tsi.point;
+		// 		contact.normal = tsi.normal;
+		// 		contact.contactDistance = tsi.point.distance(position);
+		// 		finalContacts.push(contact);
+		// 	}
+		// }
+
+		return {
+			position: position,
+			t: finalT,
+			found: found,
+			foundContacts: testTriangles
+		};
 	}
 
 	function getIntersectionTime(dt:Float, velocity:Vector) {
@@ -1137,26 +1202,36 @@ class Marble extends GameObject {
 		return 10e8;
 	}
 
-	function nudgeToContacts(position:Vector, radius:Float) {
+	function nudgeToContacts(position:Vector, radius:Float, foundContacts:Array<{
+		v:Array<Vector>,
+		n:Vector,
+		edge:Int,
+		concavity:Array<Bool>
+	}>) {
 		var it = 0;
-		var concernedContacts = this.contacts.filter(x ->
+		var concernedContacts = foundContacts; // PathedInteriors have their own nudge logic
-			(x.otherObject is src.InteriorObject && !(x.otherObject is src.PathedInterior))); // PathedInteriors have their own nudge logic
 		var prevResolved = 0;
 		do {
 			var resolved = 0;
-			for (contact in concernedContacts) {
+			for (testTri in concernedContacts) {
-				var distToContactPlane = position.dot(contact.normal) - contact.point.dot(contact.normal);
+				var tsi = Collision.TriangleSphereIntersection(testTri.v[0], testTri.v[1], testTri.v[2], testTri.n, position, radius, testTri.edge,
-				if (distToContactPlane < radius - 0.001) {
+					testTri.concavity);
-					// Nudge to the surface of the contact plane
+				if (tsi.result) {
-					position = position.add(contact.normal.multiply(radius - distToContactPlane - 0.001));
+					var distToContactPlane = tsi.point.distance(position);
-					resolved++;
+					if (radius - 0.005 - distToContactPlane > 0.0001) {
+						// Nudge to the surface of the contact plane
+						position = position.add(tsi.normal.multiply(radius - distToContactPlane - 0.005));
+						resolved++;
+					}
 				}
+
+				// var distToContactPlane = position.dot(contact.normal) - contact.point.dot(contact.normal);
 			}
 			if (resolved == 0 && prevResolved == 0)
 				break;
 			prevResolved = resolved;
 			it++;
-		} while (it < 8);
+		} while (true);
 		return position;
 	}
 
@@ -1233,10 +1308,27 @@ class Marble extends GameObject {
 				var diff = timeStep - finalPosData.t;
 				this.velocity = this.velocity.sub(A.multiply(diff));
 				this.omega = this.omega.sub(a.multiply(diff));
+				// if (finalPosData.t > 0.00001)
 				timeStep = finalPosData.t;
 			}
 			var expectedPos = finalPosData.position;
-			var newPos = nudgeToContacts(expectedPos, _radius);
+			// var newPos = expectedPos;
+			var newPos = nudgeToContacts(expectedPos, _radius, finalPosData.foundContacts);
+
+			if (this.velocity.lengthSq() > 1e-8) {
+				var posDiff = newPos.sub(expectedPos);
+				if (posDiff.lengthSq() > 1e-8) {
+					var velDiffProj = this.velocity.multiply(posDiff.dot(this.velocity) / (this.velocity.lengthSq()));
+					var expectedProjPos = expectedPos.add(velDiffProj);
+					var updatedTimestep = expectedProjPos.sub(pos).length() / velocity.length();
+
+					var tDiff = updatedTimestep - timeStep;
+					this.velocity = this.velocity.sub(A.multiply(tDiff));
+					this.omega = this.omega.sub(a.multiply(tDiff));
+
+					timeStep = updatedTimestep;
+				}
+			}
 
 			// var intersectT = intersectData.t;
 			// if (intersectData.found && intersectT > 0.001) {
@@ -1255,20 +1347,6 @@ class Marble extends GameObject {
 			// var posAdd = this.velocity.multiply(timeStep);
 			// var expectedPos = pos.add(posAdd);
 			// var newPos = nudgeToContacts(expectedPos, _radius);
-			// if (this.velocity.lengthSq() > 1e-8) {
-			// 	var posDiff = newPos.sub(expectedPos);
-			// 	if (posDiff.lengthSq() > 1e-8) {
-			// 		var velDiffProj = this.velocity.multiply(posDiff.dot(this.velocity) / (this.velocity.lengthSq()));
-			// 		var expectedProjPos = expectedPos.add(velDiffProj);
-			// 		var updatedTimestep = expectedProjPos.sub(pos).length() / velocity.length();
-
-			// 		var tDiff = updatedTimestep - timeStep;
-			// 		this.velocity = this.velocity.sub(A.multiply(tDiff));
-			// 		this.omega = this.omega.sub(a.multiply(tDiff));
-
-			// 		timeStep = updatedTimestep;
-			// 	}
-			// }
 
 			piTime += timeStep;
 			if (this.controllable) {

src/collision/Collision.hx

@@ -242,20 +242,20 @@ class Collision {
 			res.normal = P.sub(res.point).normalized();
 			res.result = true;
 
-			if (res.normal.dot(N) > 0.8) {
+			// if (res.normal.dot(N) > 0.8) {
-				// Internal edge
+			// 	// Internal edge
-				if (chosenEdge & edgeData > 0) {
+			// 	if (chosenEdge & edgeData > 0) {
-					chosenEdge -= 1;
+			// 		chosenEdge -= 1;
-					if (chosenEdge > 2)
+			// 		if (chosenEdge > 2)
-						chosenEdge--;
+			// 			chosenEdge--;
-					// if (edgeNormals[chosenEdge].length() < 0.5) {
+			// 		// if (edgeNormals[chosenEdge].length() < 0.5) {
-					//	res.normal = center.sub(res.point).normalized();
+			// 		//	res.normal = center.sub(res.point).normalized();
-					// } else
+			// 		// } else
-					if (edgeConcavities[chosenEdge]) { // Our edge is concave
+			// 		if (edgeConcavities[chosenEdge]) { // Our edge is concave
-						res.normal = N.clone();
+			// 			res.normal = N.clone();
-					}
+			// 		}
-				}
+			// 	}
-			}
+			// }
 		}
 		return res;
 	}