Push objects out of portal if it moves

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);

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);

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