Can grab objects through portals

src/physics/rigid_body.c

@@ -102,7 +102,7 @@ void rigidBodyCheckPortals(struct RigidBody* rigidBody) {
         int xorMask = i == 0 ? 0 : mask;
 
         // check if the body crossed the portal
-        if (!((~newFlags ^ xorMask) & (rigidBody->flags ^ xorMask))) {
+        if (!((~newFlags ^ xorMask) & (rigidBody->flags ^ xorMask) & mask)) {
             continue;
         }
 

src/physics/rigid_body.h

@@ -10,6 +10,7 @@ enum RigidBodyFlags {
     RigidBodyFlagsPortalsInactive = (1 << 2),
     RigidBodyFlagsCrossedPortal0 = (1 << 3),
     RigidBodyFlagsCrossedPortal1 = (1 << 4),
+    RigidBodyFlagsGrabbable = (1 << 5),
 };
 
 struct RigidBody {

src/player/player.c

@@ -9,6 +9,8 @@
 #include "physics/collision_scene.h"
 #include "physics/config.h"
 
+#define GRAB_RAYCAST_DISTANCE   3.5f
+
 struct Vector3 gGrabDistance = {0.0f, 0.0f, -2.5f};
 struct Vector3 gCameraOffset = {0.0f, 0.0f, 0.0f};
 
@@ -26,11 +28,11 @@ struct ColliderTypeData gPlayerColliderData = {
 
 void playerInit(struct Player* player) {
     collisionObjectInit(&player->collisionObject, &gPlayerColliderData, &player->body, 1.0f);
-    player->grabbing = NULL;
-    player->yaw = 0.0f;
-    player->pitch = 0.0f;
-
     player->grabbingThroughPortal = PLAYER_GRABBING_THROUGH_NOTHING;
+    player->grabbing = NULL;
+    player->pitchVelocity = 0.0f;
+    player->yawVelocity = 0.0f;
+    player->flags = 0;
 }
 
 #define PLAYER_SPEED    (5.0f)
@@ -41,6 +43,8 @@ void playerInit(struct Player* player) {
 #define ROTATE_RATE_DELTA     (M_PI * 0.25f)
 #define ROTATE_RATE_STOP_DELTA (M_PI * 0.25f)
 
+#define JUMP_IMPULSE   3.2f
+
 void playerHandleCollision(void* data, struct ContactConstraintState* contact) {
     struct Player* player = (struct Player*)data;
 
@@ -63,6 +67,30 @@ void playerApplyPortalGrab(struct Player* player, int portalIndex) {
 }
 
 void playerUpdateGrabbedObject(struct Player* player) {
+    if (controllerGetButtonDown(0, B_BUTTON)) {
+        if (player->grabbing) {
+            player->grabbing = NULL;
+        } else {
+            struct Ray ray;
+
+            ray.origin = player->body.transform.position;
+            quatMultVector(&player->body.transform.rotation, &gForward, &ray.dir);
+            vector3Negate(&ray.dir, &ray.dir);
+            
+            struct RaycastHit hit;
+
+            if (collisionSceneRaycast(&gCollisionScene, &ray, GRAB_RAYCAST_DISTANCE, 1, &hit) && hit.object->body && (hit.object->body->flags & RigidBodyFlagsGrabbable)) {
+                player->grabbing = hit.object->body;
+
+                if (hit.throughPortal) {
+                    player->grabbingThroughPortal = hit.throughPortal == gCollisionScene.portalTransforms[0] ? 0 : 1;
+                } else {
+                    player->grabbingThroughPortal = PLAYER_GRABBING_THROUGH_NOTHING;
+                }
+            }
+        }
+    }
+
     if (player->grabbing) {
         if (player->body.flags & RigidBodyFlagsCrossedPortal0) {
             playerApplyPortalGrab(player, 1);
@@ -121,6 +149,11 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
     vector3Normalize(&gForward, &gForward);
     vector3Normalize(&gRight, &gRight);
 
+
+    if ((player->flags & PlayerFlagsGrounded) && controllerGetButtonDown(0, A_BUTTON)) {
+        player->body.velocity.y = JUMP_IMPULSE;
+    }
+
     OSContPad* controllerInput = controllersGetControllerData(0);
 
     struct Vector3 targetVelocity;
@@ -151,6 +184,9 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
         vector3AddScaled(&hit.at, &gUp, PLAYER_HEAD_HEIGHT, &player->body.transform.position);
 
         player->body.velocity.y = 0.0f;
+        player->flags |= PlayerFlagsGrounded;
+    } else {
+        player->flags &= ~PlayerFlagsGrounded;
     }
 
     rigidBodyCheckPortals(&player->body);

src/player/player.h

@@ -10,15 +10,18 @@
 
 #define PLAYER_HEAD_HEIGHT              1.2f
 
+enum PlayerFlags {
+    PlayerFlagsGrounded = (1 << 0),
+};
+
 struct Player {
     struct CollisionObject collisionObject;
     struct RigidBody body;
     short grabbingThroughPortal;
     struct RigidBody* grabbing;
-    float pitch;
     float pitchVelocity;
-    float yaw;
     float yawVelocity;
+    enum PlayerFlags flags;
 };
 
 void playerInit(struct Player* player);

src/scene/cube.c

@@ -60,6 +60,8 @@ struct ColliderTypeData gCubeCollider = {
 void cubeInit(struct Cube* cube) {
     collisionObjectInit(&cube->collisionObject, &gCubeCollider, &cube->rigidBody, 1.0f);
     collisionSceneAddDynamicObject(&cube->collisionObject);
+
+    cube->collisionObject.body->flags |= RigidBodyFlagsGrabbable;
 }
 
 void cubeUpdate(struct Cube* cube) {

src/scene/scene.c

@@ -68,6 +68,18 @@ void sceneRenderWithProperties(void* data, struct RenderProps* properties, struc
 
 #define SOLID_COLOR        0, 0, 0, ENVIRONMENT, 0, 0, 0, ENVIRONMENT
 
+void sceneRenderPerformanceMetrics(struct Scene* scene, struct RenderState* renderState, struct GraphicsTask* task) {
+    gDPSetCycleType(renderState->dl++, G_CYC_1CYCLE);
+    gDPSetFillColor(renderState->dl++, (GPACK_RGBA5551(0, 0, 0, 1) << 16 | GPACK_RGBA5551(0, 0, 0, 1)));
+    gDPSetCombineMode(renderState->dl++, SOLID_COLOR, SOLID_COLOR);
+    gDPSetEnvColor(renderState->dl++, 32, 32, 32, 255);
+    gSPTextureRectangle(renderState->dl++, 32 << 2, 32 << 2, (32 + 256) << 2, (32 + 16) << 2, 0, 0, 0, 1, 1);
+    gDPPipeSync(renderState->dl++);
+    gDPSetEnvColor(renderState->dl++, 32, 255, 32, 255);
+    gSPTextureRectangle(renderState->dl++, 33 << 2, 33 << 2, (32 + 254 * scene->cpuTime / scene->lastFrameTime) << 2, (32 + 14) << 2, 0, 0, 0, 1, 1);
+    gDPPipeSync(renderState->dl++);
+}
+
 void sceneRender(struct Scene* scene, struct RenderState* renderState, struct GraphicsTask* task) {
     struct RenderProps renderProperties;
 
@@ -83,17 +95,9 @@ void sceneRender(struct Scene* scene, struct RenderState* renderState, struct Gr
     gDPSetRenderMode(renderState->dl++, G_RM_OPA_SURF, G_RM_OPA_SURF2);
     gSPGeometryMode(renderState->dl++, G_ZBUFFER | G_LIGHTING | G_CULL_BOTH, G_SHADE);
 
-    gDPSetCycleType(renderState->dl++, G_CYC_1CYCLE);
-    gDPSetFillColor(renderState->dl++, (GPACK_RGBA5551(0, 0, 0, 1) << 16 | GPACK_RGBA5551(0, 0, 0, 1)));
-    gDPSetCombineMode(renderState->dl++, SOLID_COLOR, SOLID_COLOR);
-    gDPSetEnvColor(renderState->dl++, 32, 32, 32, 255);
-    gSPTextureRectangle(renderState->dl++, 32 << 2, 32 << 2, (32 + 256) << 2, (32 + 16) << 2, 0, 0, 0, 1, 1);
-    gDPPipeSync(renderState->dl++);
-    gDPSetEnvColor(renderState->dl++, 32, 255, 32, 255);
-    gSPTextureRectangle(renderState->dl++, 33 << 2, 33 << 2, (32 + 254 * scene->cpuTime / scene->lastFrameTime) << 2, (32 + 14) << 2, 0, 0, 0, 1, 1);
-    gDPPipeSync(renderState->dl++);
+    // sceneRenderPerformanceMetrics(scene, renderState, task);
 
-    contactSolverDebugDraw(&gContactSolver, renderState);
+    // contactSolverDebugDraw(&gContactSolver, renderState);
 }
 
 unsigned ignoreInputFrames = 10;
@@ -106,15 +110,6 @@ void sceneUpdate(struct Scene* scene) {
     
     collisionSceneUpdateDynamics();
 
-    if (controllerGetButtonDown(0, B_BUTTON)) {
-        if (scene->player.grabbing) {
-            scene->player.grabbing = NULL;
-        } else {
-            scene->player.grabbing = &scene->cube.rigidBody;
-            scene->player.grabbingThroughPortal = PLAYER_GRABBING_THROUGH_NOTHING;
-        }
-    }
-
     scene->cpuTime = osGetTime() - frameStart;
     scene->lastFrameStart = frameStart;
 }