Portals work now

src/physics/collision_scene.c

@@ -55,7 +55,7 @@ void collisionObjectQueryScene(struct CollisionObject* object, struct CollisionS
 }
 
 int collisionSceneIsTouchingPortal(struct Vector3* contactPoint) {
-    if (!gCollisionScene.portalTransforms[0] || !gCollisionScene.portalTransforms[1]) {
+    if (!collisionSceneIsPortalOpen()) {
         return 0;
     }
 
@@ -76,4 +76,8 @@ int collisionSceneIsTouchingPortal(struct Vector3* contactPoint) {
     }
 
     return 0;
+}
+
+int collisionSceneIsPortalOpen() {
+    return gCollisionScene.portalTransforms[0] != NULL && gCollisionScene.portalTransforms[1] != NULL;
 }

src/physics/collision_scene.h

@@ -20,6 +20,7 @@ void collisionSceneInit(struct CollisionScene* scene, struct CollisionObject* qu
 void collisionObjectCollideWithScene(struct CollisionObject* object, struct CollisionScene* scene, struct ContactSolver* contactSolver);
 
 int collisionSceneIsTouchingPortal(struct Vector3* contactPoint);
+int collisionSceneIsPortalOpen();
 
 void collisionObjectQueryScene(struct CollisionObject* object, struct CollisionScene* scene, void* data, ManifoldCallback callback);
 

src/physics/rigid_body.c

@@ -62,19 +62,17 @@ float rigidBodyMassInverseAtLocalPoint(struct RigidBody* rigidBody, struct Vecto
 }
 
 
-int rigidBodyCheckPortals(struct RigidBody* rigidBody) {
+void rigidBodyCheckPortals(struct RigidBody* rigidBody) {
     if (!gCollisionScene.portalTransforms[0] || !gCollisionScene.portalTransforms[1]) {
         rigidBody->flags &= ~(RigidBodyFlagsInFrontPortal0 | RigidBodyFlagsInFrontPortal1);
         rigidBody->flags |= RigidBodyFlagsPortalsInactive;
-        return 0;
+        return;
     }
 
     struct Vector3 localPoint;
 
     enum RigidBodyFlags newFlags = 0;
 
-    int didTeleport = 0;
-
     for (int i = 0; i < 2; ++i) {
         transformPointInverse(gCollisionScene.portalTransforms[i], &rigidBody->transform.position, &localPoint);
 
@@ -85,8 +83,13 @@ int rigidBodyCheckPortals(struct RigidBody* rigidBody) {
         }
 
         // skip checking if portal was crossed if this is the
-        // first frame portals were active
-        if (rigidBody->flags & RigidBodyFlagsPortalsInactive || didTeleport) {
+        // first frame portals were active or the object was
+        // just teleported
+        if (rigidBody->flags & (
+            RigidBodyFlagsPortalsInactive | 
+            (RigidBodyFlagsCrossedPortal0 << (1 - i))) ||
+            (newFlags & RigidBodyFlagsCrossedPortal0)
+        ) {
             continue;
         }
 
@@ -132,12 +135,15 @@ int rigidBodyCheckPortals(struct RigidBody* rigidBody) {
 
         rigidBody->transform.rotation = newRotation;
 
-        didTeleport = 1;
-        newFlags |= RigidBodyFlagsPortalsInactive;
+        newFlags |= RigidBodyFlagsCrossedPortal0 << i;
     }
 
-    rigidBody->flags &= ~(RigidBodyFlagsInFrontPortal0 | RigidBodyFlagsInFrontPortal1 | RigidBodyFlagsPortalsInactive);
+    rigidBody->flags &= ~(
+        RigidBodyFlagsInFrontPortal0 | 
+        RigidBodyFlagsInFrontPortal1 | 
+        RigidBodyFlagsPortalsInactive |
+        RigidBodyFlagsCrossedPortal0 |
+        RigidBodyFlagsCrossedPortal1
+    );
     rigidBody->flags |= newFlags;
-
-    return didTeleport;
 }

src/physics/rigid_body.h

@@ -8,6 +8,8 @@ enum RigidBodyFlags {
     RigidBodyFlagsInFrontPortal0 = (1 << 0),
     RigidBodyFlagsInFrontPortal1 = (1 << 1),
     RigidBodyFlagsPortalsInactive = (1 << 2),
+    RigidBodyFlagsCrossedPortal0 = (1 << 3),
+    RigidBodyFlagsCrossedPortal1 = (1 << 4),
 };
 
 struct RigidBody {
@@ -34,7 +36,7 @@ void rigidBodyUpdate(struct RigidBody* rigidBody);
 void rigidBodyVelocityAtLocalPoint(struct RigidBody* rigidBody, struct Vector3* localPoint, struct Vector3* worldVelocity);
 void rigidBodyVelocityAtWorldPoint(struct RigidBody* rigidBody, struct Vector3* worldPoint, struct Vector3* worldVelocity);
 
-int rigidBodyCheckPortals(struct RigidBody* rigidBody);
+void rigidBodyCheckPortals(struct RigidBody* rigidBody);
 
 
 #endif

src/player/player.c

@@ -28,6 +28,8 @@ void playerInit(struct Player* player) {
     player->grabbing = NULL;
     player->yaw = 0.0f;
     player->pitch = 0.0f;
+
+    player->grabbingThroughPortal = PLAYER_GRABBING_THROUGH_NOTHING;
 }
 
 #define PLAYER_SPEED    (5.0f)
@@ -51,6 +53,60 @@ void playerHandleCollision(void* data, struct ContactConstraintState* contact) {
     }
 }
 
+void playerApplyPortalGrab(struct Player* player, int portalIndex) {
+    if (player->grabbingThroughPortal == PLAYER_GRABBING_THROUGH_NOTHING) {
+        player->grabbingThroughPortal = portalIndex;
+    } else if (player->grabbingThroughPortal != portalIndex) {
+        player->grabbingThroughPortal = PLAYER_GRABBING_THROUGH_NOTHING;
+    }
+}
+
+void playerUpdateGrabbedObject(struct Player* player) {
+    if (player->grabbing) {
+        if (player->body.flags & RigidBodyFlagsCrossedPortal0) {
+            playerApplyPortalGrab(player, 1);
+        }
+
+        if (player->body.flags & RigidBodyFlagsCrossedPortal1) {
+            playerApplyPortalGrab(player, 0);
+        }
+
+        if (player->grabbing->flags & RigidBodyFlagsCrossedPortal0) {
+            playerApplyPortalGrab(player, 0);
+        }
+
+        if (player->grabbing->flags & RigidBodyFlagsCrossedPortal1) {
+            playerApplyPortalGrab(player, 1);
+        }
+
+        struct Vector3 grabPoint;
+        struct Quaternion grabRotation = player->body.transform.rotation;
+
+        transformPoint(&player->body.transform, &gGrabDistance, &grabPoint);
+
+        if (player->grabbingThroughPortal != PLAYER_GRABBING_THROUGH_NOTHING) {
+            if (!collisionSceneIsPortalOpen()) {
+                // portal was closed while holding object through it
+                player->grabbing = NULL;
+                return;
+            }
+
+            struct Transform inverseA;
+            transformInvert(gCollisionScene.portalTransforms[player->grabbingThroughPortal], &inverseA);
+            struct Transform pointTransform;
+            transformConcat(gCollisionScene.portalTransforms[1 - player->grabbingThroughPortal], &inverseA, &pointTransform);
+
+            transformPoint(&pointTransform, &grabPoint, &grabPoint);
+            struct Quaternion finalRotation;
+            quatMultiply(&pointTransform.rotation, &grabRotation, &finalRotation);
+            grabRotation = finalRotation;
+        }
+
+        pointConstraintMoveToPoint(player->grabbing, &grabPoint, 20.0f);
+        pointConstraintRotateTo(player->grabbing, &grabRotation, 5.0f);
+    }
+}
+
 void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
     struct Vector3 forward;
     struct Vector3 right;
@@ -121,24 +177,9 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
 
     quatMultiply(&tempRotation, &deltaRotate, &player->body.transform.rotation);
 
-
-    // player->yaw += player->yawVelocity * FIXED_DELTA_TIME;
-    // player->pitch = clampf(player->pitch + player->pitchVelocity * FIXED_DELTA_TIME, -M_PI * 0.5f, M_PI * 0.5f);
-
-    // quatAxisAngle(&gUp, player->yaw, &transform->rotation);
-
-    // struct Quaternion pitch;
-    // quatAxisAngle(&gRight, player->pitch, &pitch);
-    // quatMultiply(&transform->rotation, &pitch, &cameraTransform->rotation);
-
     cameraTransform->rotation = player->body.transform.rotation;
 
     transformPoint(transform, &gCameraOffset, &cameraTransform->position);
 
-    if (player->grabbing) {
-        struct Vector3 grabPoint;
-        transformPoint(cameraTransform, &gGrabDistance, &grabPoint);
-        pointConstraintMoveToPoint(player->grabbing, &grabPoint, 20.0f);
-        pointConstraintRotateTo(player->grabbing, &cameraTransform->rotation, 5.0f);
-    }
+    playerUpdateGrabbedObject(player);
 }

src/player/player.h

@@ -6,9 +6,12 @@
 #include "../physics/rigid_body.h"
 #include "../physics/collision_object.h"
 
+#define PLAYER_GRABBING_THROUGH_NOTHING -1
+
 struct Player {
     struct CollisionObject collisionObject;
     struct RigidBody body;
+    short grabbingThroughPortal;
     struct RigidBody* grabbing;
     float pitch;
     float pitchVelocity;

src/scene/cube.c

@@ -69,6 +69,8 @@ void cubeUpdate(struct Cube* cube) {
 
     contactSolverSolve(&gContactSolver);
     rigidBodyUpdate(&cube->rigidBody);
+
+    rigidBodyCheckPortals(&cube->rigidBody);
 }
 
 void cubeRender(struct Cube* cube, struct RenderState* renderState) {

src/scene/scene.c

@@ -110,6 +110,7 @@ void sceneUpdate(struct Scene* scene) {
             scene->player.grabbing = NULL;
         } else {
             scene->player.grabbing = &scene->cube.rigidBody;
+            scene->player.grabbingThroughPortal = PLAYER_GRABBING_THROUGH_NOTHING;
         }
     }