replace CCD function with what MBG used

src/Marble.hx

@@ -729,6 +729,301 @@ class Marble extends GameObject {
 		rPitch.value = pitch;
 	}
 
+	function testMove(velocity:Vector, position:Vector, deltaT:Float, radius:Float, testPIs:Bool):{position:Vector, t:Float} {
+		var velLen = velocity.length();
+		if (velLen < 0.001)
+			return {position: position, t: deltaT};
+
+		var velocityDir = velocity.normalized();
+
+		var deltaPosition = velocity.multiply(deltaT);
+		var finalPosition = position.add(deltaPosition);
+
+		var searchbox = new Bounds();
+		searchbox.addSpherePos(this.x, this.y, this.z, _radius);
+		searchbox.addSpherePos(this.x + velocity.x * deltaT * 2, this.y + velocity.y * deltaT * 2, this.z + velocity.z * deltaT * 2, _radius);
+
+		var foundObjs = this.level.collisionWorld.boundingSearch(searchbox);
+
+		var finalT = deltaT;
+		var marbleCollisionTime = finalT;
+		var marbleCollisionNormal = new Vector(0, 0, 1);
+
+		var lastContactPos = new Vector();
+
+		function toDifPoint(vec:Vector) {
+			return new Point3F(vec.x, vec.y, vec.z);
+		}
+		function fromDifPoint(vec:Point3F) {
+			return new Vector(vec.x, vec.y, vec.z);
+		}
+
+		var contactPoly:{v0:Vector, v:Vector, v2:Vector};
+
+		for (obj in foundObjs) {
+			// Its an MP so bruh
+
+			var invMatrix = @:privateAccess obj.invTransform;
+			if (obj.go is PathedInterior)
+				invMatrix = obj.transform.getInverse();
+			var localpos = position.clone();
+			localpos.transform(invMatrix);
+
+			var relLocalVel = velocity.sub(obj.velocity);
+			relLocalVel.transform3x3(invMatrix);
+
+			var boundThing = new Bounds();
+			boundThing.addSpherePos(localpos.x, localpos.y, localpos.z, radius * 1.1);
+			boundThing.addSpherePos(localpos.x
+				+ relLocalVel.x * deltaT * 2, localpos.y
+				+ relLocalVel.y * deltaT * 2, localpos.z
+				+ relLocalVel.z * deltaT * 2,
+				radius * 1.1);
+
+			var surfaces = obj.octree.boundingSearch(boundThing);
+
+			for (surf in surfaces) {
+				var surface:CollisionSurface = cast surf;
+
+				var i = 0;
+				while (i < surface.indices.length) {
+					var v0 = surface.points[surface.indices[i]].transformed(obj.transform);
+					var v = surface.points[surface.indices[i + 1]].transformed(obj.transform);
+					var v2 = surface.points[surface.indices[i + 2]].transformed(obj.transform);
+
+					var polyPlane = PlaneF.ThreePoints(toDifPoint(v0), toDifPoint(v), toDifPoint(v2));
+
+					// If we're going the wrong direction or not going to touch the plane, ignore...
+					if (!(polyPlane.getNormal().dot(toDifPoint(velocityDir)) > -0.001
+						|| polyPlane.getNormal().dot(toDifPoint(finalPosition)) + polyPlane.d > radius)) {
+						// Time until collision with the plane
+						var collisionTime = (radius
+							- (polyPlane.getNormal().dot(toDifPoint(position)) + polyPlane.d)) / polyPlane.getNormal().dot(toDifPoint(velocity));
+
+						// Are we going to touch the plane during this time step?
+						if (collisionTime >= 0.0 && finalT >= collisionTime) {
+							var lastVertIndex = surface.indices[surface.indices.length - 1];
+							var lastVert = surface.points[lastVertIndex];
+
+							var collisionPos = velocity.multiply(collisionTime).add(position);
+
+							var isOnEdge:Bool = false;
+
+							for (i in 0...surface.indices.length) {
+								{
+									var thisVert = surface.points[surface.indices[i]];
+									if (thisVert != lastVert) {
+										var edgePlane = PlaneF.ThreePoints(toDifPoint(thisVert).add(polyPlane.getNormal()), toDifPoint(thisVert),
+											toDifPoint(lastVert));
+										lastVert = thisVert;
+
+										// if we are on the far side of the edge
+										if (edgePlane.getNormal().dot(toDifPoint(collisionPos)) + edgePlane.d < 0.0)
+											break;
+									}
+								}
+
+								isOnEdge = i != surface.indices.length;
+							}
+
+							// If we're inside the poly, just get the position
+							if (!isOnEdge) {
+								finalT = collisionTime;
+								finalPosition = collisionPos;
+								lastContactPos = fromDifPoint(polyPlane.project(toDifPoint(collisionPos)));
+								contactPoly = {v0: v0, v: v, v2: v2};
+								i += 3;
+								continue;
+							}
+						}
+
+						// We *might* be colliding with an edge
+
+						var lastVert = surface.points[surface.indices[surface.indices.length - 1]];
+
+						if (surface.indices.length == 0) {
+							i += 3;
+							continue;
+						}
+						var radSq = radius * radius;
+						for (iter in 0...surface.indices.length) {
+							var thisVert = surface.points[surface.indices[i]];
+
+							var vertDiff = lastVert.sub(thisVert);
+							var posDiff = position.sub(thisVert);
+
+							var velRejection = vertDiff.cross(velocity);
+							var posRejection = vertDiff.cross(posDiff);
+
+							// Build a quadratic equation to solve for the collision time
+							var a = velRejection.lengthSq();
+							var halfB = posRejection.dot(velRejection);
+							var b = halfB + halfB;
+
+							var discriminant = b * b - (posRejection.lengthSq() - vertDiff.lengthSq() * radSq) * (a * 4.0);
+
+							// If it's not quadratic or has no solution, ignore this edge.
+							if (a == 0.0 || discriminant < 0.0) {
+								lastVert = thisVert;
+								continue;
+							}
+
+							var oneOverTwoA = 0.5 / a;
+							var discriminantSqrt = Math.sqrt(discriminant);
+
+							// Solve using the quadratic formula
+							var edgeCollisionTime = (discriminantSqrt - b) * oneOverTwoA;
+							var edgeCollisionTime2 = (-b - discriminantSqrt) * oneOverTwoA;
+
+							// Make sure the 2 times are in ascending order
+							if (edgeCollisionTime2 < edgeCollisionTime) {
+								var temp = edgeCollisionTime2;
+								edgeCollisionTime2 = edgeCollisionTime;
+								edgeCollisionTime = temp;
+							}
+
+							// If the collision doesn't happen on this time step, ignore this edge.
+							if (edgeCollisionTime2 <= 0.0001 || finalT <= edgeCollisionTime) {
+								lastVert = thisVert;
+								continue;
+							}
+
+							// Check if the collision hasn't already happened
+							if (edgeCollisionTime >= 0.0) {
+								var edgeLen = vertDiff.length();
+
+								var relativeCollisionPos = velocity.multiply(edgeCollisionTime).add(position).sub(thisVert);
+
+								var distanceAlongEdge = relativeCollisionPos.dot(vertDiff) / edgeLen;
+
+								// If the collision happens outside the boundaries of the edge, ignore this edge.
+								if (-radius > distanceAlongEdge || edgeLen + radius < distanceAlongEdge) {
+									lastVert = thisVert;
+									continue;
+								}
+
+								// If the collision is within the edge, resolve the collision and continue.
+								if (distanceAlongEdge >= 0.0 && distanceAlongEdge <= edgeLen) {
+									finalT = edgeCollisionTime;
+									finalPosition = velocity.multiply(edgeCollisionTime).add(position);
+
+									lastContactPos = vertDiff.multiply(distanceAlongEdge / edgeLen).add(thisVert);
+									contactPoly = {v0: v0, v: v, v2: v2};
+
+									lastVert = thisVert;
+									continue;
+								}
+							}
+
+							// This is what happens when we collide with a corner
+
+							var speedSq = velocity.lengthSq();
+
+							// Build a quadratic equation to solve for the collision time
+							var posVertDiff = position.sub(thisVert);
+							var halfCornerB = posVertDiff.dot(velocity);
+							var cornerB = halfCornerB + halfCornerB;
+
+							var fourA = speedSq * 4.0;
+
+							var cornerDiscriminant = cornerB * cornerB - (posVertDiff.lengthSq() - radSq) * fourA;
+
+							// If it's quadratic and has a solution ...
+							if (speedSq != 0.0 && cornerDiscriminant >= 0.0) {
+								var oneOver2A = 0.5 / speedSq;
+								var cornerDiscriminantSqrt = Math.sqrt(cornerDiscriminant);
+
+								// Solve using the quadratic formula
+								var cornerCollisionTime = (cornerDiscriminantSqrt - cornerB) * oneOver2A;
+								var cornerCollisionTime2 = (-cornerB - cornerDiscriminantSqrt) * oneOver2A;
+
+								// Make sure the 2 times are in ascending order
+								if (cornerCollisionTime2 < cornerCollisionTime) {
+									var temp = cornerCollisionTime2;
+									cornerCollisionTime2 = cornerCollisionTime;
+									cornerCollisionTime = temp;
+								}
+
+								// If the collision doesn't happen on this time step, ignore this corner
+								if (cornerCollisionTime2 > 0.0001 && finalT > cornerCollisionTime) {
+									// Adjust to make sure very small negative times are counted as zero
+									if (cornerCollisionTime <= 0.0 && cornerCollisionTime > -0.0001)
+										cornerCollisionTime = 0.0;
+
+									// Check if the collision hasn't already happened
+									if (cornerCollisionTime >= 0.0) {
+										// Resolve it and continue
+										finalT = cornerCollisionTime;
+										contactPoly = {v0: v0, v: v, v2: v2};
+										finalPosition = velocity.multiply(cornerCollisionTime).add(position);
+										lastContactPos = thisVert;
+									}
+								}
+							}
+
+							// We still need to check the other corner ...
+							// Build one last quadratic equation to solve for the collision time
+							var lastVertDiff = position.sub(lastVert);
+							var lastCornerHalfB = lastVertDiff.dot(velocity);
+							var lastCornerB = lastCornerHalfB + lastCornerHalfB;
+							var lastCornerDiscriminant = lastCornerB * lastCornerB - (lastVertDiff.lengthSq() - radSq) * fourA;
+
+							// If it's not quadratic or has no solution, then skip this corner
+							if (speedSq == 0.0 || lastCornerDiscriminant < 0.0) {
+								lastVert = thisVert;
+								continue;
+							}
+
+							var lastCornerOneOver2A = 0.5 / speedSq;
+							var lastCornerDiscriminantSqrt = Math.sqrt(lastCornerDiscriminant);
+
+							// Solve using the quadratic formula
+							var lastCornerCollisionTime = (lastCornerDiscriminantSqrt - lastCornerB) * lastCornerOneOver2A;
+							var lastCornerCollisionTime2 = (-lastCornerB - lastCornerDiscriminantSqrt) * lastCornerOneOver2A;
+
+							// Make sure the 2 times are in ascending order
+							if (lastCornerCollisionTime2 < lastCornerCollisionTime) {
+								var temp = lastCornerCollisionTime2;
+								lastCornerCollisionTime2 = lastCornerCollisionTime;
+								lastCornerCollisionTime = temp;
+							}
+
+							// If the collision doesn't happen on this time step, ignore this corner
+							if (lastCornerCollisionTime2 <= 0.0001 || finalT <= lastCornerCollisionTime) {
+								lastVert = thisVert;
+								continue;
+							}
+
+							// Adjust to make sure very small negative times are counted as zero
+							if (lastCornerCollisionTime <= 0.0 && lastCornerCollisionTime > -0.0001)
+								lastCornerCollisionTime = 0.0;
+
+							// Check if the collision hasn't already happened
+							if (lastCornerCollisionTime < 0.0) {
+								lastVert = thisVert;
+								continue;
+							}
+
+							// Resolve it and continue
+							finalT = lastCornerCollisionTime;
+							finalPosition = velocity.multiply(lastCornerCollisionTime).add(position);
+							lastContactPos = lastVert;
+							contactPoly = {v0: v0, v: v, v2: v2};
+
+							lastVert = thisVert;
+						}
+					}
+
+					i += 3;
+				}
+			}
+		}
+
+		position = finalPosition;
+
+		return {position: position, t: finalT};
+	}
+
 	function getIntersectionTime(dt:Float, velocity:Vector) {
 		var searchbox = new Bounds();
 		searchbox.addSpherePos(this.x, this.y, this.z, _radius);
@@ -757,6 +1052,8 @@ class Marble extends GameObject {
 			var radius = _radius;
 
 			var invMatrix = @:privateAccess obj.invTransform;
+			if (obj.go is PathedInterior)
+				invMatrix = obj.transform.getInverse();
 			var localpos = position.clone();
 			localpos.transform(invMatrix);
 
@@ -795,12 +1092,13 @@ class Marble extends GameObject {
 					foundTriangles.push(v2);
 					// foundTriangles.push(surfacenormal);
 
-					traceinfo.resetTrace(position.clone(), position.add(velocity.multiply(dt)), this._radius);
+					traceinfo.resetTrace(position.clone(), position.add(relVelocity.multiply(dt)), this._radius);
 					traceinfo.traceSphereTriangle(v2, v, v0);
 
 					if (traceinfo.collision) {
 						var tcolpos = traceinfo.getTraceEndpoint();
-						var closest = Collision.ClosestPtPointTriangle(tcolpos, _radius, v2, v, v0, surfacenormal);
+						var closest = Collision.ClosestPtPointTriangle(tcolpos, _radius, v2.add(obj.velocity.multiply(traceinfo.t)),
+							v.add(obj.velocity.multiply(traceinfo.t)), v0.add(obj.velocity.multiply(traceinfo.t)), surfacenormal);
 						if (closest != null) {
 							var dist = tcolpos.sub(closest);
 							var distlen = dist.length();
@@ -846,10 +1144,26 @@ class Marble extends GameObject {
 
 			var pt = GJK.gjk(sph, chull);
 
-			if (pt != null) {
+			while (pt != null) {
+				if (pt.lengthSq() < 0.0001) {
+					break;
+				}
+				trace('Separating Vector Len: ${pt.length()}');
 				finalPos = finalPos.sub(pt);
+				sph.position = finalPos;
+				pt = GJK.gjk(sph, chull);
 			}
 
+			// if (pt != null) {
+			// 	finalPos = finalPos.sub(pt);
+			// 	sph.position = finalPos;
+			// 	pt = GJK.gjk(sph, chull);
+			// 	if (pt != null) {
+			// 		trace("?????");
+			// 	}
+			// 	trace('Separating Vector Len: ${pt.length()}');
+			// }
+
 			// var colpos = finalPos;
 			// var msh = new h3d.prim.Sphere();
 			// var prim = new h3d.scene.Mesh(msh);
@@ -887,7 +1201,7 @@ class Marble extends GameObject {
 			if (timeRemaining <= 0)
 				break;
 
-			var timeStep = 0.002;
+			var timeStep = 0.004;
 			if (timeRemaining < timeStep)
 				timeStep = timeRemaining;
 
@@ -898,16 +1212,18 @@ class Marble extends GameObject {
 				}
 			}
 
-			var intersectT = this.getIntersectionTime(timeStep, velocity);
+			var intersectData = testMove(velocity, this.getAbsPos().getPosition(), timeStep, _radius, true); // this.getIntersectionTime(timeStep, velocity);
+			var intersectT = intersectData.t;
 
-			if (intersectT < timeStep && intersectT >= 0.00001) {
+			if (intersectT < timeStep && intersectT >= 0.0001) {
 				// trace('CCD AT t = ${intersectT}');
-				intersectT *= 0.8; // We uh tick the shit to not actually at the contact time cause bruh
+				// intersectT *= 0.8; // We uh tick the shit to not actually at the contact time cause bruh
 				// intersectT /= 2;
 				var diff = timeStep - intersectT;
 				// this.velocity = this.velocity.sub(A.multiply(diff));
 				// this.omega = this.omega.sub(a.multiply(diff));
 				timeStep = intersectT;
+				// this.setPosition(intersectData.position.x, intersectData.position.y, intersectData.position.z);
 			}
 
 			var tempState = timeState.clone();