Get raycasting and player gravity working

src/math/ray.c

@@ -0,0 +1,12 @@
+#include "ray.h"
+
+void rayTransform(struct Transform* transform, struct Ray* ray, struct Ray* output) {
+    transformPoint(transform, &ray->origin, &output->origin);
+    quatMultVector(&transform->rotation, &ray->dir, &output->dir);
+}
+
+float rayDetermineDistance(struct Ray* ray, struct Vector3* point) {
+    struct Vector3 relative;
+    vector3Sub(point, &ray->origin, &relative);
+    return vector3Dot(&relative, &ray->dir);
+}

src/math/ray.h

@@ -0,0 +1,15 @@
+#ifndef __RAY_H__
+#define __RAY_H__
+
+#include "vector3.h"
+#include "transform.h"
+
+struct Ray {
+    struct Vector3 origin;
+    struct Vector3 dir;
+};
+
+void rayTransform(struct Transform* transform, struct Ray* ray, struct Ray* output);
+float rayDetermineDistance(struct Ray* ray, struct Vector3* point);
+
+#endif

src/physics/collision.h

@@ -4,6 +4,7 @@
 #include "../math/vector3.h"
 #include "../math/transform.h"
 #include "../math/plane.h"
+#include "../math/ray.h"
 #include "contact_solver.h"
 #include "collision_quad.h"
 
@@ -20,15 +21,21 @@ struct ContactPoint {
 };
 
 struct ColliderTypeData;
+struct CollisionObject;
+struct RaycastHit;
 
 typedef float (*MomentOfInertiaCalculator)(struct ColliderTypeData* typeData, float mass);
 
+
 typedef int (*CollideWithPlane)(void* data, struct Transform* transform, struct Plane* plane, struct ContactConstraintState* contact);
 typedef int (*CollideWithQuad)(void* data, struct Transform* transform, struct CollisionQuad* quad, struct ContactConstraintState* contact);
 
+typedef int (*RaycastCollider)(struct CollisionObject* object, struct Ray* ray, float maxDistance, struct RaycastHit* contact);
+
 struct ColliderCallbacks {
     CollideWithPlane collideWithPlane;
     CollideWithQuad collideWithQuad;
+    RaycastCollider raycast;
     MomentOfInertiaCalculator mofICalculator;
 };
 

src/physics/collision_box.c

@@ -1,10 +1,13 @@
 #include "collision_box.h"
 
+#include "raycasting.h"
+
 #include <math.h>
 
 struct ColliderCallbacks gCollisionBoxCallbacks = {
     collisionBoxCollidePlane,
     collisionBoxCollideQuad,
+    raycastBox,
     collisionBoxSolidMofI,
 };
 

src/physics/collision_scene.c

@@ -90,44 +90,57 @@ int collisionSceneIsPortalOpen() {
     return gCollisionScene.portalTransforms[0] != NULL && gCollisionScene.portalTransforms[1] != NULL;
 }
 
-#define NO_RAY_HIT_DISTANCE 1000000000000.0f
-
-int collisionSceneRaycast(struct CollisionScene* scene, struct Vector3* at, struct Vector3* dir, float maxDistance, int passThroughPortals, struct RaycastHit* hit) {
-    hit->distance = NO_RAY_HIT_DISTANCE;
+int collisionSceneRaycast(struct CollisionScene* scene, struct Ray* ray, float maxDistance, int passThroughPortals, struct RaycastHit* hit) {
+    hit->distance = maxDistance;
     hit->throughPortal = NULL;
     
     for (int i = 0; i < scene->quadCount; ++i) {
         struct RaycastHit hitTest;
 
-        if (raycastQuad(at, dir, maxDistance, scene->quads[i].collider->data, &hitTest) && hitTest.distance < hit->distance) {
+        if (raycastQuad(&scene->quads[i], ray, hit->distance, &hitTest) && hitTest.distance < hit->distance) {
             hit->at = hitTest.at;
             hit->normal = hitTest.normal;
             hit->distance = hitTest.distance;
-            hit->object = &scene->quads[i];
+            hit->object = hitTest.object;
+        }
+    }
+
+    for (int i = 0; i < scene->dynamicObjectCount; ++i) {
+        struct RaycastHit hitTest;
+
+        struct CollisionObject* object = scene->dynamicObjects[i];
+
+        if (object->collider->callbacks->raycast && 
+            object->collider->callbacks->raycast(object, ray, hit->distance, &hitTest) &&
+            hitTest.distance < hit->distance) {
+            hit->at = hitTest.at;
+            hit->normal = hitTest.normal;
+            hit->distance = hitTest.distance;
+            hit->object = hitTest.object;
         }
     }
 
     if (passThroughPortals && 
-        hit->distance != NO_RAY_HIT_DISTANCE &&
+        hit->distance != maxDistance &&
         collisionSceneIsPortalOpen()) {
         for (int i = 0; i < 2; ++i) {
             if (collisionSceneIsTouchingSinglePortal(&hit->at, gCollisionScene.portalTransforms[i])) {
                 struct Transform portalTransform;
                 collisionSceneGetPortalTransform(i, &portalTransform);
 
-                struct Vector3 newStart;
-                struct Vector3 newDir;
+                struct Ray newRay;
 
-                transformPoint(&portalTransform, &hit->at, &newStart);
-                quatMultVector(&portalTransform.rotation, dir, &newDir);
+                transformPoint(&portalTransform, &hit->at, &newRay.origin);
+                quatMultVector(&portalTransform.rotation, &ray->dir, &newRay.dir);
 
                 struct RaycastHit newHit;
 
-                int result = collisionSceneRaycast(scene, &newStart, &newDir, maxDistance - hit->distance, 0, &newHit);
+                int result = collisionSceneRaycast(scene, &newRay, maxDistance - hit->distance, 0, &newHit);
 
                 if (result) {
                     newHit.distance += hit->distance;
                     newHit.throughPortal = gCollisionScene.portalTransforms[i];
+                    *hit = newHit;
                 }
 
                 return result;
@@ -135,7 +148,7 @@ int collisionSceneRaycast(struct CollisionScene* scene, struct Vector3* at, stru
         }
     }
 
-    return hit->distance != NO_RAY_HIT_DISTANCE;
+    return hit->distance != maxDistance;
 }
 
 void collisionSceneGetPortalTransform(int fromPortal, struct Transform* out) {

src/physics/collision_scene.h

@@ -3,6 +3,7 @@
 
 #include "collision_object.h"
 #include "raycasting.h"
+#include "../math/ray.h"
 
 #define PORTAL_THICKNESS        0.11f
 #define PORTAL_X_RADIUS         0.5f
@@ -29,7 +30,7 @@ int collisionSceneIsPortalOpen();
 
 void collisionObjectQueryScene(struct CollisionObject* object, struct CollisionScene* scene, void* data, ManifoldCallback callback);
 
-int collisionSceneRaycast(struct CollisionScene* scene, struct Vector3* at, struct Vector3* dir, float maxDistance, int passThroughPortals, struct RaycastHit* hit);
+int collisionSceneRaycast(struct CollisionScene* scene, struct Ray* ray, float maxDistance, int passThroughPortals, struct RaycastHit* hit);
 
 void collisionSceneGetPortalTransform(int fromPortal, struct Transform* out);
 

src/physics/collision_sphere.c

@@ -101,5 +101,6 @@ float collisionSphereSolidMofI(struct ColliderTypeData* typeData, float mass) {
 struct ColliderCallbacks gCollisionSphereCallbacks = {
     collisionSphereCollidePlane,
     collisionSphereCollideQuad,
+    NULL, // TODO
     collisionSphereSolidMofI,
 };

src/physics/raycasting.c

@@ -1,30 +1,106 @@
 #include "raycasting.h"
 
 #include "math/mathf.h"
+#include "collision_box.h"
 
+#define NEAR_EDGE_ZERO      0.001f
 #define NEAR_DOT_ZERO       0.00001f
 #define MIN_RAY_LENGTH      0.05f
 
-int raycastQuad(struct Vector3* from, struct Vector3* dir, float maxDistance, struct CollisionQuad* quad, struct RaycastHit* contact) {
-    float normalDot = vector3Dot(dir, &quad->plane.normal);
+int raycastQuad(struct CollisionObject* quadObject, struct Ray* ray, float maxDistance, struct RaycastHit* contact) {
+    struct CollisionQuad* quad = (struct CollisionQuad*)quadObject->collider->data;
+
+    float normalDot = vector3Dot(&ray->dir, &quad->plane.normal);
 
     if (fabsf(normalDot) < NEAR_DOT_ZERO) {
         return 0;
     }
 
-    contact->distance = -(vector3Dot(from, &quad->plane.normal) + quad->plane.d) / normalDot;
+    contact->distance = -(vector3Dot(&ray->origin, &quad->plane.normal) + quad->plane.d) / normalDot;
 
     if (contact->distance < MIN_RAY_LENGTH || contact->distance > maxDistance) {
         return 0;
     }
 
-    vector3AddScaled(from, dir, contact->distance, &contact->at);
+    vector3AddScaled(&ray->origin, &ray->dir, contact->distance, &contact->at);
 
     if (collisionQuadDetermineEdges(&contact->at, quad)) {
         return 0;
     }
 
     contact->normal = quad->plane.normal;
+    contact->object = quadObject;
 
     return 1;
+}
+
+int raycastBox(struct CollisionObject* boxObject, struct Ray* ray, float maxDistance, struct RaycastHit* contact) {
+    struct CollisionBox* box = (struct CollisionBox*)boxObject->collider->data;
+
+    float distance = rayDetermineDistance(ray, &boxObject->body->transform.position);
+
+    struct Vector3 nearestPoint;
+
+    vector3AddScaled(&ray->origin, &ray->dir, distance, &nearestPoint);
+
+    if (vector3DistSqrd(&boxObject->body->transform.position, &nearestPoint) > vector3MagSqrd(&box->sideLength)) {
+        return 0;
+    }
+
+    struct Transform boxInverse;
+    transformInvert(&boxObject->body->transform, &boxInverse);
+
+    struct Ray localRay;
+    rayTransform(&boxInverse, ray, &localRay);
+
+    contact->distance = maxDistance;
+
+    for (int i = 0; i < 3; ++i) {
+        struct RaycastHit hitTest;
+
+        // d = -(o * N + d) / (N * D)
+
+        float dir = VECTOR3_AS_ARRAY(&localRay.dir)[i];
+        
+        float denominator = fabsf(dir);
+
+        if (denominator < NEAR_DOT_ZERO) {
+            continue;
+        }
+
+        float numerator = VECTOR3_AS_ARRAY(&localRay.origin)[i] - VECTOR3_AS_ARRAY(&box->sideLength)[i];
+
+        if (dir > 0) {
+            numerator = -numerator;
+        }
+
+        hitTest.distance = numerator / denominator;
+
+        // check if hit is within valid bounds
+        if (hitTest.distance < MIN_RAY_LENGTH || hitTest.distance > contact->distance) {
+            continue;
+        }
+
+        vector3AddScaled(&localRay.origin, &localRay.dir, hitTest.distance, &hitTest.at);
+
+        // check if hit is on cube face
+        if (fabsf(hitTest.at.x) > box->sideLength.x + NEAR_EDGE_ZERO ||
+            fabsf(hitTest.at.y) > box->sideLength.y + NEAR_EDGE_ZERO ||
+            fabsf(hitTest.at.z) > box->sideLength.z + NEAR_EDGE_ZERO) {
+            continue;
+        }
+
+        contact->at = hitTest.at;
+        contact->distance = hitTest.distance;
+        contact->normal = gZeroVec;
+        VECTOR3_AS_ARRAY(&contact->normal)[i] = dir < 0.0f ? 1.0f : -1.0f;
+    }
+
+    if (contact->distance != maxDistance) {
+        contact->object = boxObject;
+        transformPoint(&boxObject->body->transform, &contact->at, &contact->at);
+        quatMultVector(&boxObject->body->transform.rotation, &contact->normal, &contact->normal);
+    }
+
+    return contact->distance != maxDistance;
 }

src/physics/raycasting.h

@@ -3,6 +3,7 @@
 
 #include "collision_quad.h"
 #include "collision_object.h"
+#include "../math/ray.h"
 
 struct RaycastHit {
     struct Vector3 at;
@@ -12,6 +13,7 @@ struct RaycastHit {
     struct Transform* throughPortal;
 };
 
-int raycastQuad(struct Vector3* from, struct Vector3* dir, float maxDistance, struct CollisionQuad* quad, struct RaycastHit* contact);
+int raycastQuad(struct CollisionObject* quadObject, struct Ray* ray, float maxDistance, struct RaycastHit* contact);
+int raycastBox(struct CollisionObject* boxObject, struct Ray* ray, float maxDistance, struct RaycastHit* contact);
 
 #endif

src/player/player.c

@@ -7,6 +7,7 @@
 #include "../math/mathf.h"
 #include "physics/collision_sphere.h"
 #include "physics/collision_scene.h"
+#include "physics/config.h"
 
 struct Vector3 gGrabDistance = {0.0f, 0.0f, -2.5f};
 struct Vector3 gCameraOffset = {0.0f, 0.0f, 0.0f};
@@ -127,20 +128,26 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
     vector3Scale(&forward, &targetVelocity, -controllerInput->stick_y * PLAYER_SPEED / 80.0f);
     vector3AddScaled(&targetVelocity, &right, controllerInput->stick_x * PLAYER_SPEED / 80.0f, &targetVelocity);
 
+    targetVelocity.y = player->body.velocity.y;
+
     vector3MoveTowards(
         &player->body.velocity, 
         &targetVelocity, 
         vector3Dot(&player->body.velocity, &targetVelocity) > 0.0f ? PLAYER_ACCEL * FIXED_DELTA_TIME : PLAYER_STOP_ACCEL * FIXED_DELTA_TIME, 
         &player->body.velocity
     );
+
+    player->body.velocity.y += GRAVITY_CONSTANT * FIXED_DELTA_TIME;
+
     vector3AddScaled(&transform->position, &player->body.velocity, FIXED_DELTA_TIME, &transform->position);
 
     collisionObjectQueryScene(&player->collisionObject, &gCollisionScene, player, playerHandleCollision);
 
     struct RaycastHit hit;
-    struct Vector3 down;
-    vector3Scale(&gUp, &down, -1.0f);
-    if (collisionSceneRaycast(&gCollisionScene, &player->body.transform.position, &down, PLAYER_HEAD_HEIGHT, 1, &hit)) {
+    struct Ray ray;
+    ray.origin = player->body.transform.position;
+    vector3Scale(&gUp, &ray.dir, -1.0f);
+    if (collisionSceneRaycast(&gCollisionScene, &ray, PLAYER_HEAD_HEIGHT, 1, &hit)) {
         vector3AddScaled(&hit.at, &gUp, PLAYER_HEAD_HEIGHT, &player->body.transform.position);
 
         player->body.velocity.y = 0.0f;