Work on sphere collision

src/levels/levels.c

@@ -2,10 +2,8 @@
 
 #include "../build/assets/test_chambers/level_list.h"
 
-struct CollisionObject* levelsGetCollisionObjects() {
-    return test_chamber_00_test_chamber_00_0_collision_objects;
-}
+#include "physics/collision_scene.h"
 
-int levelsGetCollisionObjectCount() {
-    return TEST_CHAMBER_00_TEST_CHAMBER_00_0_QUAD_COLLIDERS_COUNT;
+void levelLoadCollisionScene() {
+    collisionSceneInit(&gCollisionScene, test_chamber_00_test_chamber_00_0_collision_objects, TEST_CHAMBER_00_TEST_CHAMBER_00_0_QUAD_COLLIDERS_COUNT);
 }

src/levels/levels.h

@@ -3,7 +3,6 @@
 
 #include "physics/collision_object.h"
 
-struct CollisionObject* levelsGetCollisionObjects();
-int levelsGetCollisionObjectCount();
+void levelLoadCollisionScene();
 
 #endif

src/main.c

@@ -11,6 +11,8 @@
 #include "string.h"
 #include "controls/controller.h"
 
+#include "levels/levels.h"
+
 #ifdef WITH_DEBUGGER
 #include "../debugger/debugger.h"
 #endif
@@ -130,6 +132,7 @@ static void gameProc(void* arg) {
     sceneInit(&gScene);
     romInit();
     controllersInit();
+    levelLoadCollisionScene();
 #ifdef WITH_DEBUGGER
     OSThread* debugThreads[2];
     debugThreads[0] = &gameThread;

src/physics/collision.h

@@ -10,6 +10,7 @@
 enum CollisionShapeType {
     CollisionShapeTypeBox,
     CollisionShapeTypeQuad,
+    CollisionShapeTypeSphere,
 };
 
 struct ContactPoint {

src/physics/collision_quad.c

@@ -17,20 +17,9 @@
  * edge a -->
  */
 
-int _collsionBuildQuadContact(struct Transform* boxTransform, struct CollisionQuad* quad, struct Vector3* point, struct ContactConstraintState* output, int id) {
-    struct Vector3 worldPoint;
-    struct ContactState* contact = &output->contacts[output->contactCount];
-
-    quatMultVector(&boxTransform->rotation, point, &contact->rb);
-    vector3Add(&contact->rb, &boxTransform->position, &worldPoint);
-    float penetration = planePointDistance(&quad->plane, &worldPoint);
-
-    if (penetration >= NEGATIVE_PENETRATION_BIAS) {
-        return POINT_NO_OVERLAP;
-    }
-
+int collisionQuadDetermineEdges(struct Vector3* worldPoint, struct CollisionQuad* quad) {
     struct Vector3 relative;
-    vector3Sub(&worldPoint, &quad->corner, &relative);
+    vector3Sub(worldPoint, &quad->corner, &relative);
     float edgeDistance = vector3Dot(&relative, &quad->edgeA);
 
     int edgeMask = 0;
@@ -53,6 +42,23 @@ int _collsionBuildQuadContact(struct Transform* boxTransform, struct CollisionQu
         edgeMask |= 1 << 3;
     }
 
+    return edgeMask;
+}
+
+int _collsionBuildQuadContact(struct Transform* boxTransform, struct CollisionQuad* quad, struct Vector3* point, struct ContactConstraintState* output, int id) {
+    struct Vector3 worldPoint;
+    struct ContactState* contact = &output->contacts[output->contactCount];
+
+    quatMultVector(&boxTransform->rotation, point, &contact->rb);
+    vector3Add(&contact->rb, &boxTransform->position, &worldPoint);
+    float penetration = planePointDistance(&quad->plane, &worldPoint);
+
+    if (penetration >= NEGATIVE_PENETRATION_BIAS) {
+        return POINT_NO_OVERLAP;
+    }
+
+    int edgeMask = collisionQuadDetermineEdges(&worldPoint, quad);
+
     if (edgeMask) {
         return edgeMask;
     }
@@ -389,6 +395,7 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C
 
     struct Vector3 nextFurthestPoint = deepestCorner;
 
+    // TODO roll up into a loop
     int nextId = id ^ (1 << minAxis);
     VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis];
     int pointEdges = _collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId);
@@ -438,6 +445,7 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C
          * edge a -->
          */
 
+        // TODO roll up into a loop
         if (edges & (1 << 0)) {
             edge.origin = localOrigin;
             edge.direction = localEdgeB;

src/physics/collision_quad.h

@@ -19,6 +19,7 @@ struct CollisionQuad {
     u8 enabledEdges;
 };
 
+int collisionQuadDetermineEdges(struct Vector3* worldPoint, struct CollisionQuad* quad);
 int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactConstraintState* output);
 
 #endif

src/physics/collision_scene.c

@@ -0,0 +1,14 @@
+#include "collision_scene.h"
+
+struct CollisionScene gCollisionScene;
+
+void collisionSceneInit(struct CollisionScene* scene, struct CollisionObject* quads, int quadCount) {
+    scene->quads = quads;
+    scene->quadCount = quadCount;
+}
+
+void collisionObjectCollideWithScene(struct CollisionObject* object, struct CollisionScene* scene, struct ContactSolver* contactSolver) {
+    for (int i = 0; i < scene->quadCount; ++i) {
+        collisionObjectCollideWithQuad(object, &scene->quads[i], contactSolver);
+    }
+}

src/physics/collision_scene.h

@@ -0,0 +1,16 @@
+#ifndef __COLLISION_SCENE_H__
+#define __COLLISION_SCENE_H__
+
+#include "collision_object.h"
+
+struct CollisionScene {
+    struct CollisionObject* quads;
+    int quadCount;
+};
+
+extern struct CollisionScene gCollisionScene;
+
+void collisionSceneInit(struct CollisionScene* scene, struct CollisionObject* quads, int quadCount);
+void collisionObjectCollideWithScene(struct CollisionObject* object, struct CollisionScene* scene, struct ContactSolver* contactSolver);
+
+#endif

src/physics/collision_sphere.c

@@ -0,0 +1,105 @@
+#include "collision_sphere.h"
+
+#include "math/plane.h"
+#include "math/mathf.h"
+#include "collision_quad.h"
+
+int collisionSphereCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactConstraintState* output) {
+    struct CollisionSphere* sphere = (struct CollisionSphere*)data;
+
+    float overlap = planePointDistance(&quad->plane, &boxTransform->position) - sphere->radius;
+
+    if (overlap > NEGATIVE_PENETRATION_BIAS || overlap < -2.0 * sphere->radius) {
+        return 0;
+    }
+
+    struct Vector3 relativePos;
+    vector3Sub(&boxTransform->position, &quad->corner, &relativePos);
+
+    float aLerp = clampf(vector3Dot(&relativePos, &quad->edgeA), 0.0f, quad->edgeALength);
+    float bLerp = clampf(vector3Dot(&relativePos, &quad->edgeB), 0.0f, quad->edgeBLength);
+
+    struct ContactState* contact = &output->contacts[output->contactCount];
+
+    vector3AddScaled(&quad->corner, &quad->edgeA, aLerp, &contact->ra);
+    vector3AddScaled(&contact->ra, &quad->edgeB, bLerp, &contact->ra);
+
+    vector3Sub(&boxTransform->position, &contact->rb, &output->normal);
+
+    float outputLength = vector3MagSqrd(&output->normal);
+
+    float extraRadius = sphere->radius + NEGATIVE_PENETRATION_BIAS;
+
+    if (outputLength * outputLength > extraRadius * extraRadius || vector3Dot(&output->normal, &quad->plane.normal) < 0.0f) {
+        return 0;
+    }
+
+    vector3Scale(&output->normal, &output->normal, 1.0f / sqrtf(outputLength));
+
+    vector3AddScaled(&boxTransform->position, &output->normal, -sphere->radius, &contact->rb);
+
+    output->restitution = 0.1f;
+    output->friction = 0.5f;
+    ++output->contactCount;
+    // TODO
+    output->tangentVectors[0] = quad->edgeA;
+    output->tangentVectors[1] = quad->edgeB;
+
+    contact->id = 0;
+    contact->penetration = overlap;
+    contact->bias = 0;
+    contact->normalMass = 0;
+    contact->tangentMass[0] = 0.0f;
+    contact->tangentMass[1] = 0.0f;
+    contact->normalImpulse = 0.0f;
+    contact->tangentImpulse[0] = 0.0f;
+    contact->tangentImpulse[1] = 0.0f;
+
+    return 1;
+}
+
+int collisionSphereCollidePlane(void* data, struct Transform* boxTransform, struct Plane* plane, struct ContactConstraintState* output) {
+    struct CollisionSphere* sphere = (struct CollisionSphere*)data;
+
+    float overlap = planePointDistance(plane, &boxTransform->position) - sphere->radius;
+
+    if (overlap > NEGATIVE_PENETRATION_BIAS || overlap < -2.0 * sphere->radius) {
+        return 0;
+    }
+
+    struct ContactState* contact = &output->contacts[output->contactCount];
+
+    planeProjectPoint(plane, &boxTransform->position, &contact->ra);
+    output->normal = plane->normal;
+    vector3AddScaled(&boxTransform->position, &output->normal, -sphere->radius, &contact->rb);
+
+    output->restitution = 0.1f;
+    output->friction = 0.5f;
+    ++output->contactCount;
+    // TODO
+    output->tangentVectors[0] = gRight;
+    output->tangentVectors[1] = gForward;
+
+    contact->id = 0;
+    contact->penetration = overlap;
+    contact->bias = 0;
+    contact->normalMass = 0;
+    contact->tangentMass[0] = 0.0f;
+    contact->tangentMass[1] = 0.0f;
+    contact->normalImpulse = 0.0f;
+    contact->tangentImpulse[0] = 0.0f;
+    contact->tangentImpulse[1] = 0.0f;
+
+    return 1;
+}
+
+float collisionSphereSolidMofI(struct ColliderTypeData* typeData, float mass) {
+    struct CollisionSphere* sphere = (struct CollisionSphere*)typeData->data;
+    return (2.0f / 5.0f) * mass * sphere->radius * sphere->radius;
+}
+
+struct ColliderCallbacks gCollisionSphereCallbacks = {
+    collisionSphereCollidePlane,
+    collisionSphereCollideQuad,
+    collisionSphereSolidMofI,
+};

src/physics/collision_sphere.h

@@ -0,0 +1,17 @@
+#ifndef __COLLISION_SPHERE_H__
+#define __COLLISION_SPHERE_H__
+
+#include "collision.h"
+
+extern struct ColliderCallbacks gCollisionSphereCallbacks;
+
+struct CollisionSphere {
+    float radius;
+};
+
+int collisionSphereCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactConstraintState* output);
+int collisionSphereCollidePlane(void* data, struct Transform* boxTransform, struct Plane* plane, struct ContactConstraintState* contact);
+float collisionSphereSolidMofI(struct ColliderTypeData* typeData, float mass);
+
+
+#endif

src/scene/cube.c

@@ -4,6 +4,7 @@
 #include "defs.h"
 #include "../graphics/debug_render.h"
 #include "levels/levels.h"
+#include "physics/collision_scene.h"
 
 struct CollisionBox gCubeCollisionBox = {
     {0.3165f, 0.3165f, 0.3165f}
@@ -64,12 +65,7 @@ void cubeUpdate(struct Cube* cube) {
     // collisionObjectCollideWithPlane(&cube->collisionObject, &gFloorObject, &gContactSolver);
     // collisionObjectCollideWithQuad(&cube->collisionObject, &gFloatingQuadObject, &gContactSolver);
 
-    int colliderCount = levelsGetCollisionObjectCount();
-    struct CollisionObject* objects = levelsGetCollisionObjects();
-
-    for (int i = 0; i < colliderCount; ++i) {
-        collisionObjectCollideWithQuad(&cube->collisionObject, &objects[i], &gContactSolver);
-    }
+    collisionObjectCollideWithScene(&cube->collisionObject, &gCollisionScene, &gContactSolver);
 
     contactSolverSolve(&gContactSolver);
     rigidBodyUpdate(&cube->rigidBody);