Fix CCD, attempt camera raycast shit

src/CameraController.hx

@@ -145,9 +145,40 @@ class CameraController extends Object {
 		directionVec.transform(orientationQuat.toMatrix());
 
 		var targetpos = this.marble.getAbsPos().getPosition();
-		this.x = targetpos.x + directionVec.x;
-		this.y = targetpos.y + directionVec.y;
-		this.z = targetpos.z + directionVec.z;
-		this.level.scene.camera.follow = {pos: this, target: this.marble};
+
+		var toPos = targetpos.add(directionVec);
+		camera.pos = toPos;
+		camera.target = targetpos;
+
+		var closeness = 0.1;
+		var rayCastOrigin = targetpos.add(up.multiply(marble._radius));
+		var rayCastDirection = camera.pos.sub(rayCastOrigin);
+		rayCastDirection = rayCastDirection.add(rayCastDirection.normalized().multiply(2));
+
+		var raycastresults = level.collisionWorld.rayCast(rayCastOrigin, rayCastDirection.normalized());
+		var firstHit = null;
+		var minT = 1e8;
+		for (result in raycastresults) {
+			var ca = result.point.sub(camera.pos);
+			var ba = rayCastOrigin.sub(camera.pos);
+			var t = (ba.x != 0 ? ca.x / ba.x : (ba.y != 0 ? ca.y / ba.y : (ba.z != 0 ? ca.z / ba.z : -1)));
+			if (t > 0 && t < 1 && t < minT) {
+				minT = t;
+				firstHit = result;
+			}
+		}
+		if (firstHit != null) {
+			if (firstHit.distance < CameraDistance) {
+				directionVec = directionVec.normalized().multiply(firstHit.distance);
+			}
+		}
+
+		var toPos = targetpos.add(directionVec);
+		camera.pos = toPos;
+
+		// this.x = targetpos.x + directionVec.x;
+		// this.y = targetpos.y + directionVec.y;
+		// this.z = targetpos.z + directionVec.z;
+		// this.level.scene.camera.follow = {pos: this, target: this.marble};
 	}
 }

src/Marble.hx

@@ -526,13 +526,17 @@ class Marble extends GameObject {
 
 					var surfacenormal = surface.normals[surface.indices[i]].transformed3x3(obj.transform);
 
-					var t = (-position.dot(surfacenormal) - polyPlane.d) / velocity.dot(surfacenormal);
+					var closest = Collision.IntersectTriangleCapsule(position, position.add(velocity.multiply(dt)), _radius, v0, v, v2, surfacenormal);
 
-					var pt = position.add(velocity.multiply(t));
+					if (closest != null) {
+						var t = (-position.dot(surfacenormal) - polyPlane.d) / velocity.dot(surfacenormal);
 
-					if (Collision.PointInTriangle(pt, v0, v, v2)) {
-						if (t > 0 && t < intersectT) {
-							intersectT = t;
+						var pt = position.add(velocity.multiply(t));
+
+						if (Collision.PointInTriangle(pt, v0, v, v2)) {
+							if (t > 0 && t < intersectT) {
+								intersectT = t;
+							}
 						}
 					}
 

src/collision/Collision.hx

@@ -261,6 +261,18 @@ class Collision {
 		return !(u.dot(w) < 0);
 	}
 
+	public static function IntersectTriangleCapsule(start:Vector, end:Vector, radius:Float, p1:Vector, p2:Vector, p3:Vector, normal:Vector) {
+		var dir = end.sub(start);
+		var d = -(p1.dot(normal));
+		var t = -(start.dot(normal) - d) / dir.dot(normal);
+		if (t > 1)
+			t = 1;
+		if (t < 0)
+			t = 0;
+		var tracePoint = start.add(dir.multiply(t));
+		return IntersectTriangleSphere(p1, p2, p3, normal, tracePoint, radius);
+	}
+
 	private static function GetLowestRoot(a:Float, b:Float, c:Float, max:Float):Null<Float> {
 		// check if solution exists
 		var determinant:Float = b * b - 4.0 * a * c;

src/collision/CollisionEntity.hx

@@ -55,8 +55,18 @@ class CollisionEntity implements IOctreeObject {
 	}
 
 	public function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector):Bool {
-		// TEMP cause bruh
-		return boundingBox.rayIntersection(Ray.fromValues(rayOrigin.x, rayOrigin.y, rayOrigin.z, rayDirection.x, rayDirection.y, rayDirection.z), true) != -1;
+		var invMatrix = transform.clone();
+		invMatrix.invert();
+		var rStart = rayOrigin.transformed(invMatrix);
+		var rDir = rayDirection.transformed(invMatrix);
+		var intersections = octree.raycast(rStart, rDir);
+		for (i in intersections) {
+			i.point.transform(transform);
+		}
+		if (intersections.length > 0) {
+			intersectionPoint.load(intersections[0].point);
+		}
+		return intersections.length > 0;
 	}
 
 	public function getElementType() {

src/collision/CollisionSurface.hx

@@ -76,7 +76,26 @@ class CollisionSurface implements IOctreeObject {
 	}
 
 	public function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector):Bool {
-		// TEMP cause bruh
-		return true;
+		var intersections = [];
+		var i = 0;
+		while (i < indices.length) {
+			var p1 = points[indices[i]];
+			var p2 = points[indices[i + 1]];
+			var p3 = points[indices[i + 2]];
+			var n = normals[indices[i]];
+			var d = -p1.dot(n);
+
+			var t = -(rayOrigin.dot(n) + d) / (rayDirection.dot(n));
+			var ip = rayOrigin.add(rayDirection.multiply(t));
+			if (Collision.PointInTriangle(ip, p1, p2, p3)) {
+				intersections.push(ip);
+			}
+			i += 3;
+		}
+		intersections.sort((a, b) -> cast(a.distance(rayOrigin) - b.distance(rayOrigin)));
+		if (intersections.length > 0) {
+			intersectionPoint.load(intersections[0]);
+		}
+		return intersections.length > 0;
 	}
 }

src/collision/CollisionWorld.hx

@@ -73,6 +73,10 @@ class CollisionWorld {
 		return contacts;
 	}
 
+	public function rayCast(rayStart:Vector, rayDirection:Vector) {
+		return this.octree.raycast(rayStart, rayDirection);
+	}
+
 	public function addEntity(entity:CollisionEntity) {
 		this.octree.insert(entity);
 		this.entities.push(entity);

src/collision/SphereCollisionEntity.hx

@@ -27,6 +27,7 @@ class SphereCollisionEntity extends CollisionEntity {
 
 	public override function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector):Bool {
 		// TEMP cause bruh
+		return false;
 		return boundingBox.rayIntersection(Ray.fromValues(rayOrigin.x, rayOrigin.y, rayOrigin.z, rayDirection.x, rayDirection.y, rayDirection.z), true) != -1;
 	}