Portals now work for the player

src/physics/collision_scene.c

@@ -67,7 +67,8 @@ int collisionSceneIsTouchingPortal(struct Vector3* contactPoint) {
             continue;
         }
 
-        localPoint.x *= 2.0f;
+        localPoint.x *= (1.0f / PORTAL_X_RADIUS);
+        localPoint.z = 0.0f;
 
         if (vector3MagSqrd(&localPoint) < 1.0f) {
             return 1;

src/physics/collision_scene.h

@@ -3,7 +3,8 @@
 
 #include "collision_object.h"
 
-#define PORTAL_THICKNESS    0.11f
+#define PORTAL_THICKNESS        0.11f
+#define PORTAL_X_RADIUS         0.5f
 
 struct CollisionScene {
     struct CollisionObject* quads;

src/physics/rigid_body.c

@@ -2,6 +2,7 @@
 #include "../util/time.h"
 #include "../physics/config.h"
 #include "contact_solver.h"
+#include "collision_scene.h"
 #include "defs.h"
 #include <math.h>
 
@@ -15,6 +16,8 @@ void rigidBodyInit(struct RigidBody* rigidBody, float mass, float momentOfIniter
 
     rigidBody->momentOfInertia = momentOfIniteria;
     rigidBody->momentOfInertiaInv = 1.0f / rigidBody->momentOfInertia;
+
+    rigidBody->flags = 0;
 }
 
 void rigidBodyAppyImpulse(struct RigidBody* rigidBody, struct Vector3* worldPoint, struct Vector3* impulse) {
@@ -56,4 +59,85 @@ float rigidBodyMassInverseAtLocalPoint(struct RigidBody* rigidBody, struct Vecto
     struct Vector3 crossPoint;
     vector3Cross(localPoint, normal, &crossPoint);
     return rigidBody->massInv + rigidBody->momentOfInertiaInv * vector3MagSqrd(&crossPoint);
+}
+
+
+int rigidBodyCheckPortals(struct RigidBody* rigidBody) {
+    if (!gCollisionScene.portalTransforms[0] || !gCollisionScene.portalTransforms[1]) {
+        rigidBody->flags &= ~(RigidBodyFlagsInFrontPortal0 | RigidBodyFlagsInFrontPortal1);
+        rigidBody->flags |= RigidBodyFlagsPortalsInactive;
+        return 0;
+    }
+
+    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);
+
+        int mask = (1 << i);
+
+        if (localPoint.z < 0.0f) {
+            newFlags |= mask;
+        }
+
+        // skip checking if portal was crossed if this is the
+        // first frame portals were active
+        if (rigidBody->flags & RigidBodyFlagsPortalsInactive || didTeleport) {
+            continue;
+        }
+
+        // 0 !newFlags & flags
+        // 1 newFlags & !flags
+
+        // the xorMask changes which direction
+        // each portal needs to be crossed in 
+        // order to transmit an object
+        int xorMask = i == 0 ? 0 : mask;
+
+        // check if the body crossed the portal
+        if (!((~newFlags ^ xorMask) & (rigidBody->flags ^ xorMask))) {
+            continue;
+        }
+
+        struct Vector3 scaledPoint;
+
+        scaledPoint.x = localPoint.x * (1.0f / PORTAL_X_RADIUS);
+        scaledPoint.y = localPoint.y;
+        scaledPoint.z = 0.0f;
+
+        if (vector3MagSqrd(&scaledPoint) > 1.0f) {
+            continue;
+        }
+
+        struct Transform* otherPortal = gCollisionScene.portalTransforms[1 - i];
+
+        transformPoint(otherPortal, &localPoint, &rigidBody->transform.position);
+
+        struct Quaternion inverseARotation;
+        quatConjugate(&gCollisionScene.portalTransforms[i]->rotation, &inverseARotation);
+
+        struct Quaternion rotationTransfer;
+        quatMultiply(&inverseARotation, &otherPortal->rotation, &rotationTransfer);
+
+        quatMultVector(&rotationTransfer, &rigidBody->velocity, &rigidBody->velocity);
+        quatMultVector(&rotationTransfer, &rigidBody->angularVelocity, &rigidBody->angularVelocity);
+
+        struct Quaternion newRotation;
+
+        quatMultiply(&rotationTransfer, &rigidBody->transform.rotation, &newRotation);
+
+        rigidBody->transform.rotation = newRotation;
+
+        didTeleport = 1;
+        newFlags |= RigidBodyFlagsPortalsInactive;
+    }
+
+    rigidBody->flags &= ~(RigidBodyFlagsInFrontPortal0 | RigidBodyFlagsInFrontPortal1 | RigidBodyFlagsPortalsInactive);
+    rigidBody->flags |= newFlags;
+
+    return didTeleport;
 }

src/physics/rigid_body.h

@@ -4,6 +4,12 @@
 #include "../math/transform.h"
 #include "collision.h"
 
+enum RigidBodyFlags {
+    RigidBodyFlagsInFrontPortal0 = (1 << 0),
+    RigidBodyFlagsInFrontPortal1 = (1 << 1),
+    RigidBodyFlagsPortalsInactive = (1 << 2),
+};
+
 struct RigidBody {
     struct Transform transform;
     struct Vector3 velocity;
@@ -18,6 +24,8 @@ struct RigidBody {
     // for sphere 2/5 * m * r * r
     float momentOfInertia;
     float momentOfInertiaInv;
+    
+    enum RigidBodyFlags flags;
 };
 
 void rigidBodyInit(struct RigidBody* rigidBody, float mass, float momentOfIniteria);
@@ -26,5 +34,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);
+
 
 #endif

src/player/player.c

@@ -74,15 +74,17 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
     vector3AddScaled(&targetVelocity, &right, controllerInput->stick_x * PLAYER_SPEED / 80.0f, &targetVelocity);
 
     vector3MoveTowards(
-        &player->velocity, 
+        &player->body.velocity, 
         &targetVelocity, 
-        vector3Dot(&player->velocity, &targetVelocity) > 0.0f ? PLAYER_ACCEL * FIXED_DELTA_TIME : PLAYER_STOP_ACCEL * FIXED_DELTA_TIME, 
-        &player->velocity
+        vector3Dot(&player->body.velocity, &targetVelocity) > 0.0f ? PLAYER_ACCEL * FIXED_DELTA_TIME : PLAYER_STOP_ACCEL * FIXED_DELTA_TIME, 
+        &player->body.velocity
     );
-    vector3AddScaled(&transform->position, &player->velocity, FIXED_DELTA_TIME, &transform->position);
+    vector3AddScaled(&transform->position, &player->body.velocity, FIXED_DELTA_TIME, &transform->position);
 
     collisionObjectQueryScene(&player->collisionObject, &gCollisionScene, player, playerHandleCollision);
 
+    rigidBodyCheckPortals(&player->body);
+
     float targetYaw = 0.0f;
     float targetPitch = 0.0f;
 
@@ -109,14 +111,27 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
         player->pitchVelocity * targetPitch > 0.0f ? ROTATE_RATE_DELTA : ROTATE_RATE_STOP_DELTA
     );
 
-    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);
+    struct Quaternion deltaRotate;
+    quatAxisAngle(&gUp, player->yawVelocity * FIXED_DELTA_TIME, &deltaRotate);
 
-    quatAxisAngle(&gUp, player->yaw, &transform->rotation);
+    struct Quaternion tempRotation;
+    quatMultiply(&deltaRotate, &player->body.transform.rotation, &tempRotation);
 
-    struct Quaternion pitch;
-    quatAxisAngle(&gRight, player->pitch, &pitch);
-    quatMultiply(&transform->rotation, &pitch, &cameraTransform->rotation);
+    quatAxisAngle(&gRight, player->pitchVelocity * FIXED_DELTA_TIME, &deltaRotate);
+
+    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);
 

src/player/player.h

@@ -10,7 +10,6 @@ struct Player {
     struct CollisionObject collisionObject;
     struct RigidBody body;
     struct RigidBody* grabbing;
-    struct Vector3 velocity;
     float pitch;
     float pitchVelocity;
     float yaw;