Fix quad collision bugs

src/graphics/debug_render.c

@@ -0,0 +1,46 @@
+#include "debug_render.h"
+
+#include <ultra64.h>
+
+#include "defs.h"
+#include "../math/vector3.h"
+#include "../math/matrix.h"
+
+Vtx vtx_quad[] = {
+    {{{0, 0, 0}, 0, {0, 0}, {200, 0, 0, 255}}},
+    {{{SCENE_SCALE, 0, 0}, 0, {0, 0}, {200, 0, 0, 255}}},
+    {{{SCENE_SCALE, SCENE_SCALE, 0}, 0, {0, 0}, {200, 0, 0, 255}}},
+    {{{0, SCENE_SCALE, 0}, 0, {0, 0}, {200, 0, 0, 255}}},
+};
+
+#define SOLID_SHADE_COLOR   0, 0, 0, SHADE, 0, 0, 0, SHADE
+
+Gfx mat_quad[] = {
+    gsDPSetRenderMode(G_RM_AA_ZB_OPA_SURF, G_RM_AA_ZB_OPA_SURF2),
+    gsSPGeometryMode(G_LIGHTING | G_CULL_BOTH, G_ZBUFFER | G_SHADE),
+    gsDPSetCombineMode(SOLID_SHADE_COLOR, SOLID_SHADE_COLOR),
+	gsSPEndDisplayList(),
+};
+
+Gfx gfx_quad[] = {
+    gsSPVertex(vtx_quad, 4, 0),
+    gsSP2Triangles(0, 1, 2, 0, 0, 2, 3, 0),
+	gsSPEndDisplayList(),
+};
+
+void debugRenderQuad(struct Vector3* origin, struct Vector3* edgeA, struct Vector3* edgeB, float edgeLengthA, float edgeLengthB, struct RenderState* renderState) {
+    Mtx* mtx = renderStateRequestMatrices(renderState, 1);
+    
+    struct Vector3 normal;
+    struct Vector3 x;
+    struct Vector3 y;
+    vector3Scale(edgeA, &x, edgeLengthA);
+    vector3Scale(edgeB, &y, edgeLengthB);
+    vector3Cross(edgeA, edgeB, &normal);
+    matrixFromBasisL(mtx, origin, &x, &y, &normal);
+
+    gSPMatrix(renderState->dl++, mtx, G_MTX_MUL | G_MTX_PUSH | G_MTX_MODELVIEW);
+    gSPDisplayList(renderState->dl++, mat_quad);
+    gSPDisplayList(renderState->dl++, gfx_quad);
+    gSPPopMatrix(renderState->dl++, G_MTX_MODELVIEW);
+}

src/graphics/debug_render.h

@@ -0,0 +1,9 @@
+#ifndef __DEBUG_RENDER_H__
+#define __DEBUG_RENDER_H__
+
+#include "../math/vector3.h"
+#include "renderstate.h"
+
+void debugRenderQuad(struct Vector3* origin, struct Vector3* edgeA, struct Vector3* edgeB, float edgeLengthA, float edgeLengthB, struct RenderState* renderState);
+
+#endif

src/math/matrix.c

@@ -1,6 +1,7 @@
 #include "matrix.h"
 
 #include <ultra64.h>
+#include "defs.h"
 
 void matrixPerspective(float matrix[4][4], unsigned short* perspNorm, float l, float r, float t, float b, float near, float far) {
 	guMtxIdentF(matrix);
@@ -31,4 +32,33 @@ void matrixVec3Mul(float matrix[4][4], struct Vector3* input, struct Vector4* ou
     output->y = matrix[0][1] * input->x + matrix[1][1] * input->y + matrix[2][1] * input->z + matrix[3][1];
     output->z = matrix[0][2] * input->x + matrix[1][2] * input->y + matrix[2][2] * input->z + matrix[3][2];
     output->w = matrix[0][3] * input->x + matrix[1][3] * input->y + matrix[2][3] * input->z + matrix[3][3];
+}
+
+void matrixFromBasis(float matrix[4][4], struct Vector3* origin, struct Vector3* x, struct Vector3* y, struct Vector3* z) {
+    matrix[0][0] = x->x;
+    matrix[0][1] = x->y;
+    matrix[0][2] = x->z;
+    matrix[0][3] = 0.0f;
+
+    matrix[1][0] = y->x;
+    matrix[1][1] = y->y;
+    matrix[1][2] = y->z;
+    matrix[1][3] = 0.0f;
+
+    matrix[2][0] = z->x;
+    matrix[2][1] = z->y;
+    matrix[2][2] = z->z;
+    matrix[2][3] = 0.0f;
+
+    matrix[3][0] = origin->x * SCENE_SCALE;
+    matrix[3][1] = origin->y * SCENE_SCALE;
+    matrix[3][2] = origin->z * SCENE_SCALE;
+    matrix[3][3] = 1.0f;
+}
+
+
+void matrixFromBasisL(Mtx* matrix, struct Vector3* origin, struct Vector3* x, struct Vector3* y, struct Vector3* z) {
+    float fmtx[4][4];
+    matrixFromBasis(fmtx, origin, x, y, z);
+    guMtxF2L(fmtx, matrix);
 }

src/math/matrix.h

@@ -3,9 +3,13 @@
 
 #include "vector4.h"
 #include "vector3.h"
+#include <ultra64.h>
 
 void matrixPerspective(float matrix[4][4], unsigned short* perspNorm, float l, float r, float top, float b, float near, float far);
 
 void matrixVec3Mul(float matrix[4][4], struct Vector3* input, struct Vector4* output);
 
+void matrixFromBasis(float matrix[4][4], struct Vector3* origin, struct Vector3* x, struct Vector3* y, struct Vector3* z);
+void matrixFromBasisL(Mtx* matrix, struct Vector3* origin, struct Vector3* x, struct Vector3* y, struct Vector3* z);
+
 #endif

src/physics/collision.h

@@ -5,6 +5,7 @@
 #include "../math/transform.h"
 #include "../math/plane.h"
 #include "contact_solver.h"
+#include "collision_quad.h"
 
 enum CollisionShapeType {
     CollisionShapeTypeBox,
@@ -22,9 +23,11 @@ struct ColliderTypeData;
 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);
 
 struct ColliderCallbacks {
     CollideWithPlane collideWithPlane;
+    CollideWithQuad collideWithQuad;
     MomentOfInertiaCalculator mofICalculator;
 };
 

src/physics/collision_box.c

@@ -4,6 +4,7 @@
 
 struct ColliderCallbacks gCollisionBoxCallbacks = {
     collisionBoxCollidePlane,
+    collisionBoxCollideQuad,
     collisionBoxSolidMofI,
 };
 

src/physics/collision_object.c

@@ -25,6 +25,30 @@ void collisionObjectCollideWithPlane(struct CollisionObject* object, struct Coll
             return;
         }
         
+        contactSolverAssign(contact, &localContact);
+    } else if (contact) {
+        contactSolverRemoveContact(contactSolver, contact);
+    }
+}
+
+void collisionObjectCollideWithQuad(struct CollisionObject* object, struct CollisionObject* quad, struct ContactSolver* contactSolver) {
+    CollideWithQuad quadCollider = object->collider->callbacks->collideWithQuad;
+
+    if (!quadCollider) {
+        return;
+    }
+
+    struct ContactConstraintState localContact;
+    localContact.contactCount = 0;
+
+    struct ContactConstraintState* contact = contactSolverPeekContact(contactSolver, quad, object);
+    
+    if (quadCollider(object->collider->data, &object->body->transform, quad->collider->data, &localContact)) {
+
+        if (!contact) {
+            return;
+        }
+        
         contactSolverAssign(contact, &localContact);
     } else if (contact) {
         contactSolverRemoveContact(contactSolver, contact);

src/physics/collision_object.h

@@ -12,5 +12,6 @@ struct CollisionObject {
 void collisionObjectInit(struct CollisionObject* object, struct ColliderTypeData *collider, struct RigidBody* body, float mass);
 
 void collisionObjectCollideWithPlane(struct CollisionObject* object, struct CollisionObject* plane, struct ContactSolver* contactSolver);
+void collisionObjectCollideWithQuad(struct CollisionObject* object, struct CollisionObject* quad, struct ContactSolver* contactSolver);
 
 #endif

src/physics/collision_quad.c

@@ -90,8 +90,8 @@ char secondOtherAxis[] = {
     1,
 };
 
-#define ID_FROM_AXIS_DIRS(positiveX, positiveY, positiveZ) (((positiveX) ? 0 : 1) | ((positiveY) ? 0 : 2) | ((positiveZ) ? 0 : 4))
-#define ID_SEGEMENT_FROM_AXIS(axisNumber, isPositive) ((isPositive) ? (1 << axisNumber) : 0)
+#define ID_FROM_AXIS_DIRS(positiveX, positiveY, positiveZ) (((positiveX) ? 1 : 0) | ((positiveY) ? 2 : 0) | ((positiveZ) ? 4 : 0))
+#define ID_SEGEMENT_FROM_AXIS(axisNumber, isPositive) ((isPositive) ? 0 : (1 << axisNumber))
 
 struct CollisionEdge {
     struct Vector3 origin;
@@ -162,7 +162,7 @@ void _collisionBoxCollideEdge(struct CollisionBox* box, struct Transform* boxTra
         cubeEdge.length = VECTOR3_AS_ARRAY(&box->sideLength)[axis] * 2.0f;
 
         int firstAxis = otherAxis[axis];
-        int secondAxis = otherAxis[axis];
+        int secondAxis = secondOtherAxis[axis];
 
         VECTOR3_AS_ARRAY(&cubeEdge.origin)[axis] = -VECTOR3_AS_ARRAY(&box->sideLength)[axis];
 
@@ -171,7 +171,7 @@ void _collisionBoxCollideEdge(struct CollisionBox* box, struct Transform* boxTra
             int isSecondPositive = corner & 2;
             int cornerID = ID_SEGEMENT_FROM_AXIS(firstAxis, isFirstPositive) | ID_SEGEMENT_FROM_AXIS(secondAxis, isSecondPositive);
 
-            int cornerIDMask = (1 << cornerID) | (1 << (cornerID | ID_SEGEMENT_FROM_AXIS(axis, 0)));
+            int cornerIDMask = (1 << cornerID) | (1 << (cornerID | ID_SEGEMENT_FROM_AXIS(axis, 1)));
             if ((cornerIDMask & cornerIds) == 0) {
                 continue;
             }
@@ -201,10 +201,11 @@ void _collisionBoxCollideEdge(struct CollisionBox* box, struct Transform* boxTra
                 }
 
                 quatMultVector(&boxTransform->rotation, &output->normal, &output->normal);
+                output->tangentVectors[0] = edge->direction;
+                vector3Cross(&output->normal, &edge->direction, &output->tangentVectors[1]);
             }
 
-            // TODO transform contact back into world space
-
+            // rotate from cube space to world space
             quatMultVector(&boxTransform->rotation, &contact->ra, &contact->ra);
             quatMultVector(&boxTransform->rotation, &contact->rb, &contact->rb);
 
@@ -213,7 +214,8 @@ void _collisionBoxCollideEdge(struct CollisionBox* box, struct Transform* boxTra
                 return;
             }
 
-            vector3Add(&boxTransform->position, &contact->rb, &contact->rb);
+            // move edge contact to world space
+            vector3Add(&boxTransform->position, &contact->ra, &contact->ra);
 
             ++output->contactCount;
             contact->id = 8 + axis + corner * 4;
@@ -266,8 +268,8 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C
     output->contactCount = 0;
     output->normal = quad->plane.normal;
     // TODO actually calculate tangent 
-    output->tangentVectors[0] = gRight;
-    output->tangentVectors[1] = gForward;
+    output->tangentVectors[0] = quad->edgeA;
+    output->tangentVectors[1] = quad->edgeB;
 
     output->restitution = 0.0f;
     output->friction = 1.0f;
@@ -312,25 +314,25 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C
     int nextId = id ^ (1 << minAxis);
     VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis];
     int pointEdges = _collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId);
-    edges |= pointEdges;
     if (pointEdges > 0) {
-        cornerIds |= (1 << id);
+        edges |= pointEdges;
+        cornerIds |= (1 << nextId);
     }
 
     nextId = nextId ^ (1 << midAxis);
     VECTOR3_AS_ARRAY(&nextFurthestPoint)[midAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[midAxis];
     pointEdges =_collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId);
-    edges |= pointEdges;
     if (pointEdges > 0) {
-        cornerIds |= (1 << id);
+        edges |= pointEdges;
+        cornerIds |= (1 << nextId);
     }
 
     nextId = nextId ^ (1 << minAxis);
     VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis];
     pointEdges = _collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId);
-    edges |= pointEdges;
     if (pointEdges > 0) {
-        cornerIds |= (1 << id);
+        edges |= pointEdges;
+        cornerIds |= (1 << nextId);
     }
 
     edges &= quad->enabledEdges;
@@ -346,7 +348,7 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C
         quatMultVector(&inverseBoxRotation, &quad->edgeA, &localEdgeA);
         quatMultVector(&inverseBoxRotation, &quad->edgeB, &localEdgeB);
 
-        vector3Sub(&edge.origin, &boxTransform->position, &localOrigin);
+        vector3Sub(&quad->corner, &boxTransform->position, &localOrigin);
         quatMultVector(&inverseBoxRotation, &localOrigin, &localOrigin);
 
         if (edges & (1 << 0)) {

src/scene/cube.c

@@ -2,6 +2,7 @@
 #include "../physics/collision_box.h"
 #include "../models/models.h"
 #include "defs.h"
+#include "../graphics/debug_render.h"
 
 struct CollisionBox gCubeCollisionBox = {
     {0.3165f, 0.3165f, 0.3165f}
@@ -22,6 +23,30 @@ struct CollisionObject gFloorObject = {
     NULL,
 };
 
+struct CollisionQuad gFloatingQuad = {
+    {-1.0f, 0.5f, -1.0f},
+    {1.0f, 0.0f, 0.0f},
+    2.0f,
+    {0.0f, 0.0f, 1.0f},
+    2.0f,
+    {{0.0f, 1.0f, 0.0}, -0.5f},
+    0xF,
+};
+
+struct ColliderTypeData gFloatingQuadCollider = {
+    CollisionShapeTypeQuad,
+    &gFloatingQuad,
+    0.0f,
+    1.0f,
+    NULL,
+} ;
+
+
+struct CollisionObject gFloatingQuadObject = {
+    &gFloatingQuadCollider,
+    NULL,
+};
+
 struct ColliderTypeData gCubeCollider = {
     CollisionShapeTypeBox,
     &gCubeCollisionBox,
@@ -36,6 +61,7 @@ void cubeInit(struct Cube* cube) {
 
 void cubeUpdate(struct Cube* cube) {
     collisionObjectCollideWithPlane(&cube->collisionObject, &gFloorObject, &gContactSolver);
+    collisionObjectCollideWithQuad(&cube->collisionObject, &gFloatingQuadObject, &gContactSolver);
     contactSolverSolve(&gContactSolver);
     rigidBodyUpdate(&cube->rigidBody);
 }
@@ -47,4 +73,6 @@ void cubeRender(struct Cube* cube, struct RenderState* renderState) {
     gSPMatrix(renderState->dl++, matrix, G_MTX_MODELVIEW | G_MTX_PUSH | G_MTX_MUL);
     gSPDisplayList(renderState->dl++, cube_CubeSimpleBevel_mesh);
     gSPPopMatrix(renderState->dl++, G_MTX_MODELVIEW);
+
+    debugRenderQuad(&gFloatingQuad.corner, &gFloatingQuad.edgeA, &gFloatingQuad.edgeB, gFloatingQuad.edgeALength, gFloatingQuad.edgeBLength, renderState);
 }

src/scene/scene.c

@@ -75,14 +75,19 @@ void sceneRender(struct Scene* scene, struct RenderState* renderState, struct Gr
     sceneRenderWithProperties(scene, &renderProperties, renderState);
 
     gDPPipeSync(renderState->dl++);
+
+    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);
+    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++);
 
     contactSolverDebugDraw(&gContactSolver, renderState);
 }