Camera shit

src/CameraController.hx

@@ -1,5 +1,9 @@
 package src;
 
+import h3d.col.Plane;
+import h3d.mat.Material;
+import h3d.prim.Cube;
+import h3d.scene.Mesh;
 import src.Settings;
 import hxd.Key;
 import src.Util;
@@ -61,6 +65,8 @@ class CameraController extends Object {
 	public var oob:Bool = false;
 	public var finish:Bool = false;
 
+	var originCube:Mesh;
+
 	public function new(marble:Marble) {
 		super();
 		this.marble = marble;
@@ -68,6 +74,11 @@ class CameraController extends Object {
 
 	public function init(level:MarbleWorld) {
 		this.level = level;
+		var cub = Cube.defaultUnitCube();
+		cub.addUVs();
+		cub.scale(0.2);
+		cub.addNormals();
+		originCube = new Mesh(cub, Material.create(), level.scene);
 		Window.getInstance().addEventTarget(onEvent);
 		// level.scene.addEventListener(onEvent);
 		// Sdl.setRelativeMouseMode(true);
@@ -108,7 +119,7 @@ class CameraController extends Object {
 		var rotX = deltaposX * 0.001 * Settings.controlsSettings.cameraSensitivity * Math.PI * 2;
 		var rotY = deltaposY * 0.001 * Settings.controlsSettings.cameraSensitivity * Math.PI * 2;
 		CameraYaw -= rotX;
-		CameraPitch += rotY;
+		CameraPitch -= rotY;
 		// CameraYaw = Math.PI / 2;
 		// CameraPitch = Math.PI / 4;
 
@@ -141,7 +152,7 @@ class CameraController extends Object {
 			CameraYaw += 0.75 * 5 * dt;
 		}
 
-		CameraPitch = Util.clamp(CameraPitch, -Math.PI / 2, Math.PI / 2);
+		CameraPitch = Util.clamp(CameraPitch, -Math.PI / 12, Math.PI / 2);
 
 		function getRotQuat(v1:Vector, v2:Vector) {
 			function orthogonal(v:Vector) {
@@ -176,74 +187,75 @@ class CameraController extends Object {
 
 		if (this.finish) {
 			// Make the camera spin around slowly
-			CameraPitch = Util.lerp(this.level.finishPitch, -0.45,
+			CameraPitch = Util.lerp(this.level.finishPitch, 0.45,
 				Util.clamp((this.level.timeState.currentAttemptTime - this.level.finishTime.currentAttemptTime) / 0.3, 0, 1));
-			CameraYaw = this.level.finishYaw - (this.level.timeState.currentAttemptTime - this.level.finishTime.currentAttemptTime) / -1 * 0.6;
+			CameraYaw = this.level.finishYaw - (this.level.timeState.currentAttemptTime - this.level.finishTime.currentAttemptTime) / -1.2;
 		}
 
+		var marblePosition = level.marble.getAbsPos().getPosition();
 		var up = new Vector(0, 0, 1);
 		up.transform(orientationQuat.toMatrix());
-		camera.up = up;
+		var directionVector = new Vector(1, 0, 0);
-		var upVec = new Vector(0, 0, 1);
+		var cameraVerticalTranslation = new Vector(0, 0, 0.3);
-		var quat = getRotQuat(upVec, up);
 
 		var q1 = new Quat();
 		q1.initRotateAxis(0, 1, 0, CameraPitch);
-		var q2 = new Quat();
+		directionVector.transform(q1.toMatrix());
-		q2.initRotateAxis(0, 0, 1, CameraYaw);
-
-		var dir = new Vector(1, 0, 0);
-		dir.transform(q1.toMatrix());
-		dir.transform(q2.toMatrix());
-		dir = dir.multiply(2.5);
-
-		var x = CameraDistance * Math.sin(CameraPitch) * Math.cos(CameraYaw);
-		var y = CameraDistance * Math.sin(CameraPitch) * Math.sin(CameraYaw);
-		var z = CameraDistance * Math.cos(CameraPitch);
-
-		var cameraVerticalTranslation = new Vector(0, 0, 0.3);
 		cameraVerticalTranslation.transform(q1.toMatrix());
-		cameraVerticalTranslation.transform(q2.toMatrix());
+		q1.initRotateAxis(0, 0, 1, CameraYaw);
+		directionVector.transform(q1.toMatrix());
+		cameraVerticalTranslation.transform(q1.toMatrix());
+		directionVector.transform(orientationQuat.toMatrix());
 		cameraVerticalTranslation.transform(orientationQuat.toMatrix());
-
+		camera.up = up;
-		var directionVec = dir; // new Vector(x, y, z);
+		camera.pos = marblePosition.sub(directionVector.multiply(CameraDistance));
-		directionVec.transform(orientationQuat.toMatrix());
+		camera.target = marblePosition.add(cameraVerticalTranslation);
-		// cameraVerticalTranslation.transform(orientationQuat.toMatrix());
-
-		var targetpos = this.marble.getAbsPos().getPosition();
-
-		var toPos = targetpos.add(directionVec).add(cameraVerticalTranslation);
-		camera.pos = toPos;
-		camera.target = targetpos.add(cameraVerticalTranslation); // .add(cameraVerticalTranslation);
 
 		var closeness = 0.1;
-		var rayCastOrigin = targetpos.add(up.multiply(marble._radius));
+		var rayCastOrigin = marblePosition.add(level.currentUp.multiply(marble._radius));
-		var rayCastDirection = camera.pos.sub(rayCastOrigin);
+		var rayCastDirection = camera.pos.sub(rayCastOrigin).normalized();
-		rayCastDirection = rayCastDirection.add(rayCastDirection.normalized().multiply(2));
+		var results = level.collisionWorld.rayCast(rayCastOrigin, rayCastDirection);
+		var rayCastEnd = rayCastOrigin.add(rayCastDirection.multiply(CameraDistance));
 
-		var raycastresults = level.collisionWorld.rayCast(rayCastOrigin, rayCastDirection.normalized());
 		var firstHit = null;
-		var minT = 1e8;
+		var minD = 1e8;
-		for (result in raycastresults) {
+
-			var ca = result.point.sub(camera.pos);
+		for (result in results) {
-			var ba = rayCastOrigin.sub(camera.pos);
+			if (result.distance < CameraDistance) {
-			var t = (ba.x != 0 ? ca.x / ba.x : (ba.y != 0 ? ca.y / ba.y : (ba.z != 0 ? ca.z / ba.z : -1)));
+				var t1 = (result.point.x - rayCastOrigin.x) / (rayCastEnd.x - rayCastOrigin.x);
-			if (t > 0 && t < 1 && t < minT) {
+				if (t1 < 0 || t1 > 1)
-				minT = t;
+					continue;
-				firstHit = result;
+				var t2 = (result.point.y - rayCastOrigin.y) / (rayCastEnd.y - rayCastOrigin.y);
+				if (t2 < 0 || t2 > 1)
+					continue;
+				var t3 = (result.point.z - rayCastOrigin.z) / (rayCastEnd.z - rayCastOrigin.z);
+				if (t3 < 0 || t3 > 1)
+					continue;
+				if (result.distance < minD) {
+					minD = result.distance;
+					firstHit = result;
+				}
 			}
 		}
+
 		if (firstHit != null) {
 			if (firstHit.distance < CameraDistance) {
-				directionVec = directionVec.normalized().multiply(firstHit.distance);
+				// camera.pos = marblePosition.sub(directionVector.multiply(firstHit.distance * 0.7));
+				var plane = new Plane(firstHit.normal.x, firstHit.normal.y, firstHit.normal.z, firstHit.point.dot(firstHit.normal));
+				var normal = firstHit.normal.multiply(-1);
+				// var position = firstHit.point;
+
+				var projected = plane.project(camera.pos.toPoint());
+				var dist = plane.distance(camera.pos.toPoint());
+				if (dist < closeness) {
+					camera.pos = projected.toVector().add(normal.multiply(-closeness));
+				}
 			}
 		}
 
-		var toPos = targetpos.add(directionVec);
-		camera.pos = toPos;
 		if (oob) {
 			camera.pos = lastCamPos;
-			camera.target = targetpos.add(cameraVerticalTranslation);
+			camera.target = marblePosition.add(cameraVerticalTranslation);
 		}
 
 		if (!oob)

src/Marble.hx

@@ -223,15 +223,10 @@ class Marble extends GameObject {
 	}
 
 	public function getMarbleAxis() {
-		var cammat = Matrix.I();
+		var motiondir = new Vector(0, -1, 0);
-		var xrot = new Matrix();
+		motiondir.transform(Matrix.R(0, 0, camera.CameraYaw));
-		xrot.initRotationX(this.camera.CameraPitch);
+		motiondir.transform(level.newOrientationQuat.toMatrix());
-		var zrot = new Matrix();
-		zrot.initRotationZ(this.camera.CameraYaw);
-		cammat.multiply(xrot, zrot);
-		cammat.multiply(cammat, this.level.newOrientationQuat.toMatrix());
 		var updir = this.level.currentUp;
-		var motiondir = new Vector(cammat._21, cammat._22, cammat._23);
 		var sidedir = motiondir.cross(updir);
 
 		sidedir.normalize();

src/MarbleWorld.hx

@@ -228,8 +228,8 @@ class MarbleWorld extends Scheduler {
 		this.marble.reset();
 
 		var euler = startquat.quat.toEuler();
-		this.marble.camera.CameraYaw = euler.z - Math.PI / 2;
+		this.marble.camera.CameraYaw = euler.z + Math.PI / 2;
-		this.marble.camera.CameraPitch = -0.45;
+		this.marble.camera.CameraPitch = 0.45;
 		this.marble.camera.oob = false;
 		this.marble.camera.finish = false;
 		this.marble.mode = Start;

src/collision/BoxCollisionEntity.hx

@@ -23,7 +23,7 @@ class BoxCollisionEntity extends CollisionEntity {
 		this.boundingBox.transform(this.transform);
 	}
 
-	public override function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector):Bool {
+	public override function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector, intersectionNormal: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;

src/collision/CollisionEntity.hx

@@ -59,17 +59,20 @@ class CollisionEntity implements IOctreeObject {
 		this.boundingBox = boundingBox;
 	}
 
-	public function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector):Bool {
+	public function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector, intersectionNormal:Vector):Bool {
 		var invMatrix = transform.clone();
 		invMatrix.invert();
-		var rStart = rayOrigin.transformed(invMatrix);
+		var rStart = rayOrigin.clone();
-		var rDir = rayDirection.transformed(invMatrix);
+		rStart.transform(invMatrix);
+		var rDir = rayDirection.transformed3x3(invMatrix);
 		var intersections = octree.raycast(rStart, rDir);
 		for (i in intersections) {
 			i.point.transform(transform);
+			i.normal.transform3x3(transform);
 		}
 		if (intersections.length > 0) {
 			intersectionPoint.load(intersections[0].point);
+			intersectionNormal.load(intersections[0].normal);
 		}
 		return intersections.length > 0;
 	}
@@ -141,7 +144,7 @@ class CollisionEntity implements IOctreeObject {
 								cinfo.normal = normal; // surface.normals[surface.indices[i]];
 								cinfo.point = closest;
 								// cinfo.collider = this;
-								cinfo.velocity = this.velocity;
+								cinfo.velocity = this.velocity.clone();
 								cinfo.contactDistance = radius - closest.distance(position);
 								cinfo.otherObject = this.go;
 								// cinfo.penetration = radius - (position.sub(closest).dot(normal));

src/collision/CollisionSurface.hx

@@ -75,7 +75,7 @@ class CollisionSurface implements IOctreeObject {
 		this.priority = priority;
 	}
 
-	public function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector):Bool {
+	public function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector, intersectionNormal:Vector):Bool {
 		var intersections = [];
 		var i = 0;
 		while (i < indices.length) {
@@ -87,14 +87,17 @@ class CollisionSurface implements IOctreeObject {
 
 			var t = -(rayOrigin.dot(n) + d) / (rayDirection.dot(n));
 			var ip = rayOrigin.add(rayDirection.multiply(t));
+			ip.w = 1;
 			if (Collision.PointInTriangle(ip, p1, p2, p3)) {
-				intersections.push(ip);
+				intersections.push({pt: ip, n: n});
 			}
 			i += 3;
 		}
-		intersections.sort((a, b) -> cast(a.distance(rayOrigin) - b.distance(rayOrigin)));
+		intersections.sort((a,
+				b) -> (a.pt.distance(rayOrigin) == b.pt.distance(rayOrigin)) ? 0 : (a.pt.distance(rayOrigin) > b.pt.distance(rayOrigin) ? 1 : -1));
 		if (intersections.length > 0) {
-			intersectionPoint.load(intersections[0]);
+			intersectionPoint.load(intersections[0].pt);
+			intersectionNormal.load(intersections[0].n);
 		}
 		return intersections.length > 0;
 	}

src/collision/CollisionWorld.hx

@@ -87,6 +87,8 @@ class CollisionWorld {
 
 	public function rayCast(rayStart:Vector, rayDirection:Vector) {
 		// 	return [];
+		rayStart.w = 1;
+		rayDirection.w = 1;
 		return this.octree.raycast(rayStart, rayDirection);
 	}
 

src/collision/SphereCollisionEntity.hx

@@ -26,7 +26,7 @@ class SphereCollisionEntity extends CollisionEntity {
 		this.boundingBox = boundingBox;
 	}
 
-	public override function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector):Bool {
+	public override function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector, intersectionNormal: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;

src/octree/IOctreeObject.hx

@@ -5,5 +5,5 @@ import h3d.col.Bounds;
 
 interface IOctreeObject extends IOctreeElement {
 	var boundingBox:Bounds;
-	function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector):Bool;
+	function isIntersectedByRay(rayOrigin:Vector, rayDirection:Vector, intersectionPoint:Vector, intersectionNormal:Vector):Bool;
 }

src/octree/Octree.hx

@@ -130,7 +130,7 @@ class Octree {
 	public function raycast(rayOrigin:Vector, rayDirection:Vector) {
 		var intersections:Array<OctreeIntersection> = [];
 		this.root.raycast(rayOrigin, rayDirection, intersections);
-		intersections.sort((a, b) -> cast(a.distance - b.distance));
+		intersections.sort((a, b) -> (a.distance == b.distance) ? 0 : (a.distance > b.distance ? 1 : -1));
 		return intersections;
 	}
 

src/octree/OctreeIntersection.hx

@@ -6,6 +6,7 @@ class OctreeIntersection {
 	public var object:IOctreeObject;
 	public var point:Vector;
 	public var distance:Float;
+	public var normal:Vector;
 
 	public function new() {}
 }

src/octree/OctreeNode.hx

@@ -140,11 +140,13 @@ class OctreeNode implements IOctreeElement {
 
 		for (obj in this.objects) {
 			var iSec = new Vector();
-			if (obj.isIntersectedByRay(rayOrigin, rayDirection, iSec)) {
+			var iNorm = new Vector();
+			if (obj.isIntersectedByRay(rayOrigin, rayDirection, iSec, iNorm)) {
 				var intersectionData = new OctreeIntersection();
 				intersectionData.distance = rayOrigin.distance(iSec);
 				intersectionData.object = obj;
 				intersectionData.point = iSec;
+				intersectionData.normal = iNorm;
 				intersections.push(intersectionData);
 			}
 		}