From c56d382c3ac6086dc084dedcd8b3d0915d201751 Mon Sep 17 00:00:00 2001
From: RandomityGuy <31925790+RandomityGuy@users.noreply.github.com>
Date: Mon, 13 Feb 2023 23:29:09 +0530
Subject: [PATCH] fix some CCD and trigger bugs

---
 src/Marble.hx                     |  8 +++++---
 src/MarbleWorld.hx                |  4 +++-
 src/PathedInterior.hx             | 15 ++++++++-------
 src/collision/CollisionEntity.hx  |  6 ++++--
 src/triggers/MustChangeTrigger.hx |  5 ++++-
 5 files changed, 24 insertions(+), 14 deletions(-)

diff --git a/src/Marble.hx b/src/Marble.hx
index 66e77299..e985834b 100644
--- a/src/Marble.hx
+++ b/src/Marble.hx
@@ -999,10 +999,11 @@ class Marble extends GameObject {
 
 		// for (iter in 0...10) {
 		//	var iterationFound = false;
-		for (obj in foundObjs.filter(x -> x.go is InteriorObject || (x.go is PathedInterior))) {
+		for (obj in foundObjs) {
 			// Its an MP so bruh
 			if (!obj.go.isCollideable)
 				continue;
+
 			var invMatrix = @:privateAccess obj.invTransform;
 			if (obj.go is PathedInterior)
 				invMatrix = obj.transform.getInverse();
@@ -1063,8 +1064,9 @@ class Marble extends GameObject {
 					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)
+						edge: surf.edgeData != null ? surf.edgeData[Math.floor(i / 3)] : 0,
+						concavity: surface.edgeConcavities != null ? surface.edgeConcavities.slice(Math.floor(i / 3),
+							Math.floor(i / 3) + 3) : [false, false, false],
 					});
 
 					// Time until collision with the plane
diff --git a/src/MarbleWorld.hx b/src/MarbleWorld.hx
index c77abe0d..89f51ad5 100644
--- a/src/MarbleWorld.hx
+++ b/src/MarbleWorld.hx
@@ -1606,10 +1606,12 @@ class MarbleWorld extends Scheduler {
 				for (collider in endpadBB) {
 					if (collider.go == this.endPad) {
 						var chull = cast(collider, collision.CollisionHull);
+						var chullinvT = @:privateAccess chull.invTransform.clone();
+						chullinvT.clone();
 						for (surface in chull.surfaces) {
 							var i = 0;
 							while (i < surface.indices.length) {
-								var surfaceN = surface.normals[surface.indices[i]].transformed3x3(chull.transform);
+								var surfaceN = surface.normals[surface.indices[i]].transformed3x3(chullinvT);
 								var v1 = surface.points[surface.indices[i]].transformed(chull.transform);
 								var surfaceD = -surfaceN.dot(v1);
 
diff --git a/src/PathedInterior.hx b/src/PathedInterior.hx
index 4c8ade16..605c80d6 100644
--- a/src/PathedInterior.hx
+++ b/src/PathedInterior.hx
@@ -257,9 +257,10 @@ class PathedInterior extends InteriorObject {
 		var m2:PathedInteriorMarker = this.markerData[1];
 		if (m1 == null) {
 			// Incase there are no markers at all
-			var mat = new Matrix();
-			this.baseOrientation.toMatrix(mat);
-			mat.scale(this.baseScale.x, this.baseScale.y, this.baseScale.z);
+			var tmp = new Matrix();
+			var mat = Matrix.S(this.baseScale.x, this.baseScale.y, this.baseScale.z);
+			this.baseOrientation.toMatrix(tmp);
+			mat.multiply3x4(mat, tmp);
 			mat.setPosition(this.basePosition);
 			return mat;
 		}
@@ -310,10 +311,10 @@ class PathedInterior extends InteriorObject {
 		position = position.sub(firstPosition);
 		position = position.add(basePosition); // Add the base position
 
-		var mat = new Matrix();
-		this.baseOrientation.toMatrix(mat);
-
-		mat.scale(this.baseScale.x, this.baseScale.y, this.baseScale.z);
+		var tmp = new Matrix();
+		var mat = Matrix.S(this.baseScale.x, this.baseScale.y, this.baseScale.z);
+		this.baseOrientation.toMatrix(tmp);
+		mat.multiply3x4(mat, tmp);
 		mat.setPosition(position);
 
 		return mat;
diff --git a/src/collision/CollisionEntity.hx b/src/collision/CollisionEntity.hx
index 9dfdd0c3..10daa5ce 100644
--- a/src/collision/CollisionEntity.hx
+++ b/src/collision/CollisionEntity.hx
@@ -120,6 +120,8 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
 
 	public function rayCast(rayOrigin:Vector, rayDirection:Vector):Array<RayIntersectionData> {
 		var invMatrix = invTransform;
+		var invTPos = invMatrix.clone();
+		invTPos.transpose();
 		var rStart = rayOrigin.clone();
 		rStart.transform(invMatrix);
 		var rDir = rayDirection.transformed3x3(invMatrix);
@@ -128,7 +130,7 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
 			var iData:Array<RayIntersectionData> = [];
 			for (i in intersections) {
 				i.point.transform(transform);
-				i.normal.transform3x3(transform);
+				i.normal.transform3x3(invTPos);
 				i.normal.normalize();
 				iData.push({point: i.point, normal: i.normal, object: i.object});
 			}
@@ -137,7 +139,7 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
 			var intersections = this.bvh.rayCast(rStart, rDir);
 			for (i in intersections) {
 				i.point.transform(transform);
-				i.normal.transform3x3(transform);
+				i.normal.transform3x3(invTPos);
 				i.normal.normalize();
 			}
 
diff --git a/src/triggers/MustChangeTrigger.hx b/src/triggers/MustChangeTrigger.hx
index b32ed300..09e81ddb 100644
--- a/src/triggers/MustChangeTrigger.hx
+++ b/src/triggers/MustChangeTrigger.hx
@@ -14,7 +14,10 @@ class MustChangeTrigger extends Trigger {
 	}
 
 	public override function onMarbleEnter(time:TimeState) {
-		this.interior.setTargetTime(time, MisParser.parseNumber(this.element.targettime) / 1000);
+		var ttime = MisParser.parseNumber(this.element.targettime);
+		if (ttime > 0)
+			ttime /= 1000;
+		this.interior.setTargetTime(time, ttime);
 		if (this.element.instant == "1") {
 			if (this.element.icontinuetottime != null && this.element.icontinuetottime != "0") {
 				// Absolutely strange, and not sure if it's even a thing in MBG, but is implement nonetheless.