ccd test

src/Marble.hx

@@ -44,6 +44,7 @@ import src.MarbleGame;
 import src.CameraController;
 import src.Resource;
 import h3d.mat.Texture;
+import collision.CCDCollision.TraceInfo;
 
 class Move {
 	public var d:Vector;
@@ -729,7 +730,7 @@ class Marble extends GameObject {
 	function getIntersectionTime(dt:Float, velocity:Vector) {
 		var searchbox = new Bounds();
 		searchbox.addSpherePos(this.x, this.y, this.z, _radius);
-		searchbox.addSpherePos(this.x + velocity.x * dt, this.y + velocity.y * dt, this.z + velocity.z * dt, _radius);
+		searchbox.addSpherePos(this.x + velocity.x * dt * 2, this.y + velocity.y * dt * 2, this.z + velocity.z * dt * 2, _radius);
 
 		var position = this.getAbsPos().getPosition();
 
@@ -742,6 +743,10 @@ class Marble extends GameObject {
 
 		var intersectT = 10e8;
 
+		var traceinfo = new TraceInfo();
+
+		traceinfo.resetTrace(position.clone(), position.add(velocity.multiply(dt)), this._radius);
+
 		for (obj in foundObjs) {
 			var radius = _radius;
 
@@ -754,7 +759,8 @@ class Marble extends GameObject {
 
 			var boundThing = new Bounds();
 			boundThing.addSpherePos(localpos.x, localpos.y, localpos.z, radius * 1.1);
-			boundThing.addSpherePos(localpos.x + relLocalVel.x * dt, localpos.y + relLocalVel.y * dt, localpos.z + relLocalVel.z * dt, radius * 1.1);
+			boundThing.addSpherePos(localpos.x + relLocalVel.x * dt * 2, localpos.y + relLocalVel.y * dt * 2, localpos.z + relLocalVel.z * dt * 2,
+				radius * 1.1);
 
 			var surfaces = obj.octree.boundingSearch(boundThing);
 
@@ -778,21 +784,23 @@ class Marble extends GameObject {
 
 					var surfacenormal = surface.normals[surface.indices[i]].transformed3x3(obj.transform);
 
+					traceinfo.traceSphereTriangle(v0, v, v2);
+
 					// var closest = Collision.IntersectTriangleCapsule(position, position.add(relVelocity.multiply(dt)), _radius, v0, v, v2, surfacenormal);
 					// var closest = Collision.IntersectTriangleSphere(v0, v, v2, surfacenormal, position, radius);
 
 					// if (closest != null) {
 					// This is some ballpark approximation, very bruh
-					var radiusDir = relVelocity.normalized().multiply(radius);
-					var t = (-position.add(radiusDir).dot(surfacenormal) + v0.dot(surfacenormal)) / relVelocity.dot(surfacenormal);
+					// var radiusDir = relVelocity.normalized().multiply(radius);
+					// var t = (-position.add(radiusDir).dot(surfacenormal) + v0.dot(surfacenormal)) / relVelocity.dot(surfacenormal);
 
-					var pt = position.add(radiusDir).add(relVelocity.multiply(t));
+					// var pt = position.add(radiusDir).add(relVelocity.multiply(t));
 
-					if (Collision.PointInTriangle(pt, v0, v, v2)) {
-						if (t > 0 && t < intersectT) {
-							intersectT = t;
-						}
-					}
+					// if (Collision.PointInTriangle(pt, v0, v, v2)) {
+					// 	if (t > 0 && t < intersectT) {
+					// 		intersectT = t;
+					// 	}
+					// }
 					// }
 
 					i += 3;
@@ -800,6 +808,14 @@ class Marble extends GameObject {
 			}
 		}
 
+		if (traceinfo.collision) {
+			var traceDist = traceinfo.getTraceDistance();
+			var t = traceDist / velocity.length();
+			if (t < intersectT) {
+				intersectT = t;
+			}
+		}
+
 		return intersectT;
 	}
 
@@ -838,7 +854,8 @@ class Marble extends GameObject {
 
 			var intersectT = this.getIntersectionTime(timeStep, velocity);
 
-			if (intersectT < timeStep && intersectT >= 0.001) {
+			if (intersectT < timeStep && intersectT >= 0.00001) {
+				trace('CCD AT t = ${intersectT}');
 				intersectT *= 0.8; // We uh tick the shit to not actually at the contact time cause bruh
 				// intersectT /= 2;
 				var diff = timeStep - intersectT;

src/Resource.hx

@@ -17,7 +17,7 @@ class Resource<T> {
 
 	public function acquire() {
 		this.referenceCount++;
-		trace('Acquiring Resource ${this.identifier}: ${this.referenceCount}');
+		// trace('Acquiring Resource ${this.identifier}: ${this.referenceCount}');
 	}
 
 	public function release() {
@@ -25,7 +25,7 @@ class Resource<T> {
 		if (this.referenceCount == 0) {
 			disposeFunc(this.resource);
 			this.resourceMap.remove(this.identifier);
-			trace('Releasing Resource ${this.identifier}');
+			// trace('Releasing Resource ${this.identifier}');
 		}
 	}
 }

src/collision/CCDCollision.hx

@@ -0,0 +1,276 @@
+package collision;
+
+import h3d.Vector;
+
+@:publicFields
+class TraceInfo {
+	var start:Vector;
+	var end:Vector;
+	var scaledStart:Vector;
+	var radius:Float;
+	var invRadius:Float;
+	var vel:Vector;
+	var scaledVel:Vector;
+	var velLength:Float;
+	var normVel:Vector;
+	var collision:Bool;
+	var t:Float;
+	var intersectPoint:Vector;
+	var tmp:Vector;
+
+	public function new() {
+		this.start = new Vector();
+		this.end = new Vector();
+		this.scaledStart = new Vector();
+		this.radius = 0;
+		this.invRadius = 0;
+		this.vel = new Vector();
+		this.scaledVel = new Vector();
+		this.velLength = 0;
+		this.normVel = new Vector();
+		this.collision = false;
+		this.t = 0;
+		this.intersectPoint = new Vector();
+	}
+
+	public function resetTrace(start:Vector, end:Vector, radius:Float) {
+		this.invRadius = 1 / radius;
+		this.radius = radius;
+
+		this.start.set(start.x, start.y, start.z);
+		this.end.set(end.x, end.y, end.z);
+		this.vel = this.end.sub(this.start);
+		this.normVel = this.vel.normalized();
+
+		this.scaledStart = start.multiply(this.invRadius);
+		this.scaledVel = this.vel.multiply(this.invRadius);
+
+		this.velLength = this.vel.length();
+
+		this.collision = false;
+		this.t = 1.0;
+	}
+
+	public function setCollision(t:Float, point:Vector) {
+		this.collision = true;
+		if (t < this.t) {
+			this.t = t;
+			this.intersectPoint = point.multiply(this.radius);
+		}
+	}
+
+	public function getTraceEndpoint() {
+		return this.start.add(this.vel.multiply(this.t));
+	}
+
+	public function getTraceDistance() {
+		return this.vel.multiply(this.t).length();
+	}
+
+	public function traceSphereTriangle(a:Vector, b:Vector, c:Vector) {
+		var invRadius = this.invRadius;
+		var vel = this.scaledVel;
+		var start = this.scaledStart;
+
+		// Scale the triangle points so that we're colliding against a unit-radius sphere.
+		var ta = a.multiply(invRadius);
+		var tb = b.multiply(invRadius);
+		var tc = c.multiply(invRadius);
+
+		// Calculate triangle normal.
+		// This may be better to do as a pre-process
+		var pab = tb.sub(ta);
+		var pac = tc.sub(ta);
+		var norm = pab.cross(pac);
+		norm.normalize();
+		var planeD = -(norm.dot(ta));
+
+		// Colliding against the backface of the triangle
+		if (norm.dot(this.normVel) >= 0) {
+			// Two choices at this point:
+
+			// 1) Negate the normal so that it always points towards the start point
+			// This feels kludgy, but I'm not sure if there's a better alternative
+			/*vec3.negate(norm, norm);
+				planeD = -planeD; */
+
+			// 2) Or allow it to pass through
+			return;
+		}
+
+		// Get interval of plane intersection:
+		var t0, t1;
+		var embedded = false;
+
+		// Calculate the signed distance from sphere
+		// position to triangle plane
+		var distToPlane = start.dot(norm) + planeD;
+
+		// cache this as we’re going to use it a few times below:
+		var normDotVel = norm.dot(vel);
+
+		if (normDotVel == 0.0) {
+			// Sphere is travelling parrallel to the plane:
+			if (Math.abs(distToPlane) >= 1.0) {
+				// Sphere is not embedded in plane, No collision possible
+				return;
+			} else {
+				// Sphere is completely embedded in plane.
+				// It intersects in the whole range [0..1]
+				embedded = true;
+				t0 = 0.0;
+				t1 = 1.0;
+			}
+		} else {
+			// Calculate intersection interval:
+			t0 = (-1.0 - distToPlane) / normDotVel;
+			t1 = (1.0 - distToPlane) / normDotVel;
+			// Swap so t0 < t1
+			if (t0 > t1) {
+				var temp = t1;
+				t1 = t0;
+				t0 = temp;
+			}
+			// Check that at least one result is within range:
+			if (t0 > 1.0 || t1 < 0.0) {
+				// No collision possible
+				return;
+			}
+			// Clamp to [0,1]
+			if (t0 < 0.0)
+				t0 = 0.0;
+			if (t1 < 0.0)
+				t1 = 0.0;
+			if (t0 > 1.0)
+				t0 = 1.0;
+			if (t1 > 1.0)
+				t1 = 1.0;
+		}
+
+		// If the closest possible collision point is further away
+		// than an already detected collision then there's no point
+		// in testing further.
+		if (t0 >= this.t) {
+			return;
+		}
+
+		// t0 and t1 now represent the range of the sphere movement
+		// during which it intersects with the triangle plane.
+		// Collisions cannot happen outside that range.
+
+		// Check for collision againt the triangle face:
+		if (!embedded) {
+			// Calculate the intersection point with the plane
+			var planeIntersect = start.sub(norm);
+			var v = vel.multiply(t0);
+			planeIntersect = planeIntersect.add(v);
+
+			// Is that point inside the triangle?
+			if (pointInTriangle(planeIntersect, ta, tb, tc)) {
+				this.setCollision(t0, planeIntersect);
+				// Collisions against the face will always be closer than vertex or edge collisions
+				// so we can stop checking now.
+				return;
+			}
+		}
+
+		var velSqrLen = vel.lengthSq();
+		var t = this.t;
+
+		// Check for collision againt the triangle vertices:
+		t = testVertex(ta, velSqrLen, t, start, vel);
+		t = testVertex(tb, velSqrLen, t, start, vel);
+		t = testVertex(tc, velSqrLen, t, start, vel);
+
+		// Check for collision against the triangle edges:
+		t = testEdge(ta, tb, velSqrLen, t, start, vel);
+		t = testEdge(tb, tc, velSqrLen, t, start, vel);
+		testEdge(tc, ta, velSqrLen, t, start, vel);
+	}
+
+	function pointInTriangle(p:Vector, t0:Vector, t1:Vector, t2:Vector) {
+		var pt0 = t0.sub(p);
+		var pt1 = t1.sub(p);
+		var pt2 = t2.sub(p);
+
+		pt0.normalize();
+		pt1.normalize();
+		pt2.normalize();
+
+		var a = pt0.dot(pt1);
+		var b = pt1.dot(pt2);
+		var c = pt2.dot(pt0);
+
+		var angle = Math.acos(a) + Math.acos(b) + Math.acos(c);
+
+		// If the point is on the triangle all the interior angles should add up to 360 deg.
+		var collision = Math.abs(angle - (2 * Math.PI)) < 0.01;
+		return collision;
+	}
+
+	function getLowestRoot(a:Float, b:Float, c:Float, maxR:Float) {
+		var det = b * b - 4.0 * a * c;
+		if (det < 0) {
+			return -1.0;
+		}
+
+		var sqrtDet = Math.sqrt(det);
+		var r1 = (-b - sqrtDet) / (2.0 * a);
+		var r2 = (-b + sqrtDet) / (2.0 * a);
+
+		if (r1 > r2) {
+			var tmp = r2;
+			r2 = r1;
+			r1 = tmp;
+		}
+
+		if (r1 > 0 && r1 < maxR) {
+			return r1;
+		}
+
+		if (r2 > 0 && r2 < maxR) {
+			return r2;
+		}
+
+		return -1.0;
+	}
+
+	function testVertex(p:Vector, velSqrLen:Float, t:Float, start:Vector, vel:Vector) {
+		var v = start.sub(p);
+		var b = 2.0 * vel.dot(v);
+		var c = v.lengthSq() - 1.0;
+		var newT = getLowestRoot(velSqrLen, b, c, t);
+		if (newT != -1) {
+			this.setCollision(newT, p);
+			return newT;
+		}
+		return t;
+	}
+
+	function testEdge(pa:Vector, pb:Vector, velSqrLen:Float, t:Float, start:Vector, vel:Vector) {
+		var edge = pb.sub(pa);
+		var v = pa.sub(start);
+
+		var edgeSqrLen = edge.lengthSq();
+		var edgeDotVel = edge.dot(vel);
+		var edgeDotSphereVert = edge.dot(v);
+
+		var a = edgeSqrLen * -velSqrLen + edgeDotVel * edgeDotVel;
+		var b = edgeSqrLen * (2.0 * vel.dot(v)) - 2.0 * edgeDotVel * edgeDotSphereVert;
+		var c = edgeSqrLen * (1.0 - v.lengthSq()) + edgeDotSphereVert * edgeDotSphereVert;
+
+		// Check for intersection against infinite line
+		var newT = getLowestRoot(a, b, c, t);
+		if (newT != -1 && newT < this.t) {
+			// Check if intersection against the line segment:
+			var f = (edgeDotVel * newT - edgeDotSphereVert) / edgeSqrLen;
+			if (f >= 0.0 && f <= 1.0) {
+				v = edge.multiply(f);
+				v = v.add(pa);
+				this.setCollision(newT, v);
+				return newT;
+			}
+		}
+		return t;
+	}
+}