Work on physics

src/math/quaternion.c

@@ -3,6 +3,7 @@
 #include <ultra64.h>
 #include <assert.h>
 #include "mathf.h"
+#include <math.h>
 
 void quatIdent(struct Quaternion* q) {
     q->x = 0.0f;
@@ -216,3 +217,21 @@ void quatApplyAngularVelocity(struct Quaternion* input, struct Vector3* w, float
     quatAdd(&intermediate, input, output);
     quatNormalize(output, output);
 }
+
+
+void quatDecompose(struct Quaternion* input, struct Vector3* axis, float* angle) {
+    float axisMag = sqrtf(input->x * input->x + input->y * input->y + input->z * input->z);
+
+    if (axisMag < 0.0001f) {
+        *axis = gUp;
+        *angle = 0.0f;
+        return;
+    }
+
+    float magInv = 1.0f / axisMag;
+
+    axis->x = input->x * magInv;
+    axis->y = input->y * magInv;
+    axis->z = input->z * magInv;
+    *angle = sinf(axisMag) * 2.0f;
+}

src/math/quaternion.h

@@ -25,5 +25,6 @@ void quatEulerAngles(struct Vector3* angles, struct Quaternion* out);
 // cheap approximation of slerp
 void quatLerp(struct Quaternion* a, struct Quaternion* b, float t, struct Quaternion* out);
 void quatApplyAngularVelocity(struct Quaternion* input, struct Vector3* w, float timeStep, struct Quaternion* output);
+void quatDecompose(struct Quaternion* input, struct Vector3* axis, float* angle);
 
 #endif

src/physics/point_constraint.c

@@ -0,0 +1,44 @@
+#include "point_constraint.h"
+
+#include "../util/time.h"
+
+void pointConstraintMoveToPoint(struct RigidBody* rigidBody, struct Vector3* worldPoint, float maxImpulse) {
+    vector3Sub(worldPoint, &rigidBody->transform.position, &rigidBody->velocity);
+    struct Vector3 targetVelocity;
+    vector3Scale(&rigidBody->velocity, &targetVelocity, 1.0f / FIXED_DELTA_TIME);
+
+    struct Vector3 delta;
+    vector3Sub(&targetVelocity, &rigidBody->velocity, &delta);
+
+    float deltaSqrd = vector3MagSqrd(&delta);
+    if (deltaSqrd < maxImpulse * maxImpulse) {
+        rigidBody->velocity = targetVelocity;
+    } else {
+        vector3AddScaled(&rigidBody->velocity, &delta, maxImpulse / sqrtf(deltaSqrd), &rigidBody->velocity);
+    }
+}
+
+void pointConstraintRotateTo(struct RigidBody* rigidBody, struct Quaternion* worldRotation, float maxImpulse) {
+    struct Quaternion inverseRigidBody;
+    quatConjugate(&rigidBody->transform.rotation, &inverseRigidBody);
+
+    struct Quaternion deltaAngle;
+    quatMultiply(worldRotation, &inverseRigidBody, &deltaAngle);
+
+    struct Vector3 axis;
+    float angle;
+    quatDecompose(&deltaAngle, &axis, &angle);
+
+    struct Vector3 targetVelocity;
+    vector3Scale(&axis, &targetVelocity, angle * (1.0f / FIXED_DELTA_TIME));
+
+    struct Vector3 delta;
+    vector3Sub(&targetVelocity, &rigidBody->angularVelocity, &delta);
+
+    float deltaSqrd = vector3MagSqrd(&delta);
+    if (deltaSqrd < maxImpulse * maxImpulse) {
+        rigidBody->angularVelocity = targetVelocity;
+    } else {
+        vector3AddScaled(&rigidBody->angularVelocity, &delta, maxImpulse / sqrtf(deltaSqrd), &rigidBody->angularVelocity);
+    }
+}

src/physics/point_constraint.h

@@ -0,0 +1,9 @@
+#ifndef __POINT_CONSTRAINT_H__
+#define __POINT_CONSTRAINT_H__
+
+#include "rigid_body.h"
+
+void pointConstraintMoveToPoint(struct RigidBody* rigidBody, struct Vector3* worldPoint, float maxImpulse);
+void pointConstraintRotateTo(struct RigidBody* rigidBody, struct Quaternion* worldRotation, float maxImpulse);
+
+#endif

src/player/player.c

@@ -3,24 +3,33 @@
 #include "../controls/controller.h"
 #include "../util/time.h"
 #include "../defs.h"
+#include "../physics/point_constraint.h"
+#include "../math/mathf.h"
+
+struct Vector3 gGrabDistance = {0.0f, 0.0f, -2.5f};
+struct Vector3 gCameraOffset = {0.0f, 1.2f, 0.0f};
 
 void playerInit(struct Player* player) {
     transformInitIdentity(&player->transform);
+    player->grabbing = NULL;
+    player->yaw = 0.0f;
+    player->pitch = 0.0f;
 }
 
 #define PLAYER_SPEED    (2.0f)
 
-#define ROTATE_RATE     (M_PI * 0.25f)
+#define ROTATE_RATE     (M_PI * 2.0f)
+#define ROTATE_RATE_DELTA     (M_PI * 0.25f)
 
 void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
     struct Vector3 forward;
     struct Vector3 right;
 
-    quatMultVector(&cameraTransform->rotation, &gForward, &forward);
-    quatMultVector(&cameraTransform->rotation, &gRight, &right);
+    quatMultVector(&player->transform.rotation, &gForward, &forward);
+    quatMultVector(&player->transform.rotation, &gRight, &right);
 
-    gForward.y = 0.0f;
-    gRight.y = 0.0f;
+    forward.y = 0.0f;
+    right.y = 0.0f;
 
     vector3Normalize(&gForward, &gForward);
     vector3Normalize(&gRight, &gRight);
@@ -30,20 +39,39 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
     vector3AddScaled(&player->transform.position, &forward, -controllerInput->stick_y * FIXED_DELTA_TIME * PLAYER_SPEED / 80.0f, &player->transform.position);
     vector3AddScaled(&player->transform.position, &right, controllerInput->stick_x * FIXED_DELTA_TIME * PLAYER_SPEED / 80.0f, &player->transform.position);
 
-    float rotate = 0.0f;
+    float targetYaw = 0.0f;
+    float targetPitch = 0.0f;
 
     if (controllerGetButton(0, L_CBUTTONS)) {
-        rotate += ROTATE_RATE * FIXED_DELTA_TIME;
+        targetYaw += ROTATE_RATE;
     } else if (controllerGetButton(0, R_CBUTTONS)) {
-        rotate -= ROTATE_RATE * FIXED_DELTA_TIME;
+        targetYaw -= ROTATE_RATE;
     }
 
-    if (rotate) {
-        struct Quaternion rotateBy;
-        quatAxisAngle(&gUp, rotate, &rotateBy);
-        struct Quaternion finalRotation;
+    if (controllerGetButton(0, U_CBUTTONS)) {
+        targetPitch -= ROTATE_RATE;
+    } else if (controllerGetButton(0, D_CBUTTONS)) {
+        targetPitch += ROTATE_RATE;
+    }
 
-        quatMultiply(&player->transform.rotation, &rotateBy, &finalRotation);
-        player->transform.rotation = finalRotation;
+    player->yawVelocity = mathfMoveTowards(player->yawVelocity, targetYaw, ROTATE_RATE_DELTA);
+    player->pitchVelocity = mathfMoveTowards(player->pitchVelocity, targetPitch, ROTATE_RATE_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);
+
+    quatAxisAngle(&gUp, player->yaw, &player->transform.rotation);
+
+    struct Quaternion pitch;
+    quatAxisAngle(&gRight, player->pitch, &pitch);
+    quatMultiply(&player->transform.rotation, &pitch, &cameraTransform->rotation);
+
+    transformPoint(&player->transform, &gCameraOffset, &cameraTransform->position);
+
+    if (player->grabbing) {
+        struct Vector3 grabPoint;
+        transformPoint(cameraTransform, &gGrabDistance, &grabPoint);
+        pointConstraintMoveToPoint(player->grabbing, &grabPoint, 5.0f);
+        pointConstraintRotateTo(player->grabbing, &cameraTransform->rotation, 5.0f);
     }
 }

src/player/player.h

@@ -3,9 +3,15 @@
 
 #include "../math/transform.h"
 #include "../graphics/renderstate.h"
+#include "../physics/rigid_body.h"
 
 struct Player {
     struct Transform transform;
+    struct RigidBody* grabbing;
+    float pitch;
+    float pitchVelocity;
+    float yaw;
+    float yawVelocity;
 };
 
 void playerInit(struct Player* player);

src/scene/cube.c

@@ -24,13 +24,13 @@ void cubeInit(struct Cube* cube) {
 }
 
 void cubeUpdate(struct Cube* cube) {
-    rigidBodyUpdate(&cube->rigidBody);
-
     gContactSolver.contactCount = 0;
 
     rigidBodyCollideWithPlane(&cube->rigidBody, &gFloor, &gContactSolver);
 
     contactSolverSolve(&gContactSolver);
+
+    rigidBodyUpdate(&cube->rigidBody);
 }
 
 void cubeRender(struct Cube* cube, struct RenderState* renderState) {

src/scene/scene.c

@@ -12,13 +12,13 @@
 #include "sk64/skelatool_defs.h"
 #include "controls/controller.h"
 #include "shadow_map.h"
+#include "../physics/point_constraint.h"
+#include "../controls/controller.h"
 
 #include "../levels/test_chamber_00_0/header.h"
 
 struct Vector3 gStartPosition = {5.0f, 0.0f, -5.0f};
 
-struct Vector3 gCameraOffset = {0.0f, 1.2f, 0.0f};
-
 void sceneInit(struct Scene* scene) {
     cameraInit(&scene->camera, 45.0f, 0.25f * SCENE_SCALE, 80.0f * SCENE_SCALE);
     playerInit(&scene->player);
@@ -46,7 +46,7 @@ void sceneInit(struct Scene* scene) {
     scene->cube.rigidBody.transform.position.z = -1.0f;
 
     quatAxisAngle(&gRight, M_PI * 0.125f, &scene->cube.rigidBody.transform.rotation);
-    // scene->cube.rigidBody.angularVelocity = gOneVec;
+    scene->cube.rigidBody.angularVelocity = gOneVec;
 }
 
 void sceneRenderWithProperties(void* data, struct RenderProps* properties, struct RenderState* renderState) {
@@ -75,9 +75,14 @@ void sceneRender(struct Scene* scene, struct RenderState* renderState, struct Gr
 unsigned ignoreInputFrames = 10;
 
 void sceneUpdate(struct Scene* scene) {
-    transformPoint(&scene->player.transform, &gCameraOffset, &scene->camera.transform.position);
-    scene->camera.transform.rotation = scene->player.transform.rotation;
-
     playerUpdate(&scene->player, &scene->camera.transform);
     cubeUpdate(&scene->cube);
+
+    if (controllerGetButtonDown(0, B_BUTTON)) {
+        if (scene->player.grabbing) {
+            scene->player.grabbing = NULL;
+        } else {
+            scene->player.grabbing = &scene->cube.rigidBody;
+        }
+    }
 }