Work on physics

This commit is contained in:
James Lambert 2022-03-15 20:50:02 -06:00
parent 5788dcc232
commit 06c6985ebb
8 changed files with 136 additions and 24 deletions

View File

@ -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;
@ -215,4 +216,22 @@ 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;
}

View File

@ -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

View File

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

View File

@ -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

View File

@ -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;
quatMultiply(&player->transform.rotation, &rotateBy, &finalRotation);
player->transform.rotation = finalRotation;
if (controllerGetButton(0, U_CBUTTONS)) {
targetPitch -= ROTATE_RATE;
} else if (controllerGetButton(0, D_CBUTTONS)) {
targetPitch += ROTATE_RATE;
}
}
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);
}
}

View File

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

View File

@ -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) {

View File

@ -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;
}
}
}