From 4421a479d7ad77133c907e1e720a9975c1091258 Mon Sep 17 00:00:00 2001
From: James Lambert <lambertjamesd@gmail.com>
Date: Sun, 2 Jul 2023 21:32:45 -0600
Subject: [PATCH] Push objects out of portal if it moves

---
 src/physics/collision_scene.c | 39 +++++++++++++++++++++++++++++++++++
 src/physics/collision_scene.h |  2 ++
 src/scene/scene.c             |  3 +++
 3 files changed, 44 insertions(+)

diff --git a/src/physics/collision_scene.c b/src/physics/collision_scene.c
index ba6a0f6..9ced5ee 100644
--- a/src/physics/collision_scene.c
+++ b/src/physics/collision_scene.c
@@ -196,6 +196,45 @@ int collisionSceneIsPortalOpen() {
     return gCollisionScene.portalTransforms[0] != NULL && gCollisionScene.portalTransforms[1] != NULL;
 }
 
+void collisionScenePushObjectsOutOfPortal(int portalIndex) {
+    if (!gCollisionScene.portalTransforms[portalIndex]) {
+        return;
+    }
+
+    struct Transform* portalTransform = gCollisionScene.portalTransforms[portalIndex];
+
+    struct Vector3 reversePortalNormal = gZeroVec;
+    reversePortalNormal.z = portalIndex ? -1.0f : 1.0f;
+    quatMultVector(&portalTransform->rotation, &reversePortalNormal, &reversePortalNormal);
+    
+    for (unsigned i = 0; i < gCollisionScene.dynamicObjectCount; ++i) {
+        struct CollisionObject* object = gCollisionScene.dynamicObjects[i];
+
+        if (!(object->flags & ((RigidBodyIsTouchingPortalA | RigidBodyWasTouchingPortalA) << portalIndex))) {
+            continue;
+        }
+
+        struct Vector3 colliderPoint;
+        object->collider->callbacks->minkowsiSum(
+            object->collider->data, 
+            &object->body->rotationBasis, 
+            &reversePortalNormal, 
+            &colliderPoint
+        );
+
+        struct Vector3 offset;
+        vector3Sub(&portalTransform->position, &colliderPoint, &offset);
+
+        float depth = vector3Dot(&offset, &reversePortalNormal);
+
+        if (depth > 0.0f) {
+            continue;
+        }
+
+        vector3AddScaled(&object->body->transform.position, &reversePortalNormal, depth, &object->body->transform.position);
+    }
+}
+
 void collisionSceneRaycastRoom(struct CollisionScene* scene, struct Room* room, struct Ray* ray, int collisionLayers, struct RaycastHit* hit) {
     int currX = GRID_CELL_X(room, ray->origin.x);
     int currZ = GRID_CELL_Z(room, ray->origin.z);
diff --git a/src/physics/collision_scene.h b/src/physics/collision_scene.h
index 253830f..ed3adf8 100644
--- a/src/physics/collision_scene.h
+++ b/src/physics/collision_scene.h
@@ -35,6 +35,8 @@ int collisionSceneIsTouchingSinglePortal(struct Vector3* contactPoint, struct Ve
 int collisionSceneIsTouchingPortal(struct Vector3* contactPoint, struct Vector3* contactNormal);
 int collisionSceneIsPortalOpen();
 
+void collisionScenePushObjectsOutOfPortal(int portalIndex);
+
 int collisionSceneRaycast(struct CollisionScene* scene, int roomIndex, struct Ray* ray, int collisionLayers, float maxDistance, int passThroughPortals, struct RaycastHit* hit);
 
 void collisionSceneGetPortalTransform(int fromPortal, struct Transform* out);
diff --git a/src/scene/scene.c b/src/scene/scene.c
index af37ce2..eb8b70e 100644
--- a/src/scene/scene.c
+++ b/src/scene/scene.c
@@ -735,6 +735,9 @@ int sceneOpenPortal(struct Scene* scene, struct Transform* at, int transformInde
             if (just_checking){
                 return 1;
             }
+
+            collisionScenePushObjectsOutOfPortal(portalIndex);
+
             soundPlayerPlay(soundsPortalOpen2, 1.0f, 1.0f, &at->position, &gZeroVec);
 
             // the portal position may have been adjusted