Add velcro to the box

src/physics/collision_box.c

@@ -11,7 +11,7 @@ struct ColliderCallbacks gCollisionBoxCallbacks = {
     collisionBoxSolidMofI,
 };
 
-void _collsionBuildPlaneContact(struct Transform* boxTransform, struct Plane* plane, struct Vector3* point, struct ContactConstraintState* output, int id) {
+int _collsionBuildPlaneContact(struct Transform* boxTransform, struct Plane* plane, struct Vector3* point, struct ContactConstraintState* output, int id) {
     struct Vector3 worldPoint;
     struct ContactState* contact = &output->contacts[output->contactCount];
 
@@ -20,11 +20,10 @@ void _collsionBuildPlaneContact(struct Transform* boxTransform, struct Plane* pl
     float penetration = planePointDistance(plane, &worldPoint);
 
     if (penetration >= NEGATIVE_PENETRATION_BIAS) {
-        return;
+        return 0;
     }
 
-
-    vector3AddScaled(&worldPoint, &plane->normal, penetration, &contact->ra);
+    vector3AddScaled(&worldPoint, &plane->normal, -penetration, &contact->ra);
 
     ++output->contactCount;
     contact->id = id;
@@ -33,6 +32,11 @@ void _collsionBuildPlaneContact(struct Transform* boxTransform, struct Plane* pl
     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;
 }
 
 
@@ -77,7 +81,49 @@ int collisionBoxCollidePlane(void* data, struct Transform* boxTransform, struct
     output->restitution = 0.0f;
     output->friction = 1.0f;
 
-    _collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, output, id);
+    if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, output, id)) {
+        return 0;
+    }
+
+    struct Vector3 sideLengthProjected;
+    vector3Multiply(&box->sideLength, &normalInBoxSpace, &sideLengthProjected);
+
+    int minAxis = 0;
+    float minAxisDistnace = fabsf(sideLengthProjected.x);
+    int maxAxis = 0;
+    float maxAxisDistnace = minAxisDistnace;
+
+    for (int axis = 1; axis < 3; ++axis) {
+        float length = fabsf(VECTOR3_AS_ARRAY(&sideLengthProjected)[axis]);
+
+        if (length < minAxisDistnace) {
+            minAxisDistnace = length;
+            minAxis = axis;
+        }
+
+        if (length > maxAxisDistnace) {
+            maxAxisDistnace = length;
+            maxAxis = axis;
+        }
+    }
+
+    int midAxis = 3 - maxAxis - minAxis;
+
+    struct Vector3 nextFurthestPoint = deepestCorner;
+    int nextId = id ^ (1 << minAxis);
+    VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis];
+
+    if (!_collsionBuildPlaneContact(boxTransform, plane, &nextFurthestPoint, output, nextId)) {
+        return 1;
+    }
+
+    nextId = nextId ^ (1 << midAxis);
+    VECTOR3_AS_ARRAY(&nextFurthestPoint)[midAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[midAxis];
+    _collsionBuildPlaneContact(boxTransform, plane, &nextFurthestPoint, output, nextId);
+
+    nextId = nextId ^ (1 << minAxis);
+    VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis];
+    _collsionBuildPlaneContact(boxTransform, plane, &nextFurthestPoint, output, nextId);
 
     return 1;
 }

src/physics/contact_solver.c

@@ -7,6 +7,12 @@
 
 #include <string.h>
 
+#define Q3_BAUMGARTE 0.03f
+
+#define Q3_PENETRATION_SLOP 0.01f
+
+#define ENABLE_FRICTION 1
+
 struct ContactSolver gContactSolver;
 
 void contactSolverInit(struct ContactSolver* contactSolver) {

src/physics/contact_solver.h

@@ -5,12 +5,6 @@
 
 struct CollisionObject;
 
-#define Q3_BAUMGARTE 0.2f
-
-#define Q3_PENETRATION_SLOP 0.01f
-
-#define ENABLE_FRICTION 1
-
 struct VelocityState
 {
 	struct Vector3 w;

src/physics/debug_renderer.c

@@ -0,0 +1,79 @@
+#include "debug_renderer.h"
+#include "defs.h"
+#include "collision_object.h"
+
+#define SOLID_SHADE_COLOR   0, 0, 0, SHADE, 0, 0, 0, SHADE
+
+Vtx vtx_contact_solver_debug[] = {
+    {{{0, 0, -16}, 0, {0, 0}, {255, 0, 0, 255}}},
+    {{{0, 0, 16}, 0, {0, 0}, {255, 0, 0, 255}}},
+    {{{64, 0, 0}, 0, {0, 0}, {255, 0, 0, 255}}},
+
+    {{{0, -16, 0}, 0, {0, 0}, {0, 255, 0, 255}}},
+    {{{64, 0, 0}, 0, {0, 0}, {0, 255, 0, 255}}},
+    {{{0, 16, 0}, 0, {0, 0}, {0, 255, 0, 255}}},
+};
+
+Gfx contact_solver_debug[] = {
+    gsSPVertex(vtx_contact_solver_debug, 6, 0),
+    gsSP2Triangles(0, 1, 2, 0, 3, 4, 5, 0),
+	gsSPEndDisplayList(),
+};
+
+Gfx mat_contact_solver_debug[] = {
+    gsDPSetRenderMode(G_RM_OPA_SURF, G_RM_OPA_SURF2),
+    gsSPGeometryMode(G_ZBUFFER | G_LIGHTING | G_CULL_BOTH, G_SHADE),
+    gsDPSetCombineMode(SOLID_SHADE_COLOR, SOLID_SHADE_COLOR),
+	gsSPEndDisplayList(),
+};
+
+void contactConstraintStateDebugDraw(struct ContactConstraintState* constraintState, struct RenderState* renderState) {
+    for (int i = 0; i < constraintState->contactCount; ++i) {
+        struct ContactState* contact = &constraintState->contacts[i];
+
+        float mat[4][4];
+
+        mat[0][0] = constraintState->normal.x;
+        mat[0][1] = constraintState->normal.y;
+        mat[0][2] = constraintState->normal.z;
+        mat[0][3] = 0.0f;
+
+        mat[1][0] = constraintState->tangentVectors[0].x;
+        mat[1][1] = constraintState->tangentVectors[0].y;
+        mat[1][2] = constraintState->tangentVectors[0].z;
+        mat[1][3] = 0.0f;
+
+        mat[2][0] = constraintState->tangentVectors[1].x;
+        mat[2][1] = constraintState->tangentVectors[1].y;
+        mat[2][2] = constraintState->tangentVectors[1].z;
+        mat[2][3] = 0.0f;
+
+        struct Vector3 pos = contact->ra;
+
+        if (constraintState->shapeA->body) {
+            vector3Add(&pos, &constraintState->shapeB->body->transform.position, &pos);
+        }
+
+        mat[3][0] = pos.x * SCENE_SCALE;
+        mat[3][1] = pos.y * SCENE_SCALE;
+        mat[3][2] = pos.z * SCENE_SCALE;
+        mat[3][3] = 1.0f;
+
+        Mtx* mtx = renderStateRequestMatrices(renderState, 1);
+        guMtxF2L(mat, mtx);
+
+        gSPMatrix(renderState->dl++, mtx, G_MTX_PUSH | G_MTX_MUL | G_MTX_MODELVIEW);
+        gSPDisplayList(renderState->dl++, contact_solver_debug);
+        gSPPopMatrix(renderState->dl++, G_MTX_MODELVIEW);
+    }
+}
+
+void contactSolverDebugDraw(struct ContactSolver* contactSolver, struct RenderState* renderState) {
+    gSPDisplayList(renderState->dl++, mat_contact_solver_debug);
+    struct ContactConstraintState* contact = contactSolver->activeContacts;
+
+    while (contact) {
+        contactConstraintStateDebugDraw(contact, renderState);
+        contact = contact->next;
+    }
+}

src/physics/debug_renderer.h

@@ -0,0 +1,9 @@
+#ifndef __DEBUG_RENDERER_H__
+#define __DEBUG_RENDERER_H__
+
+#include "contact_solver.h"
+#include "../graphics/renderstate.h"
+
+void contactSolverDebugDraw(struct ContactSolver* contactSolver, struct RenderState* renderState);
+
+#endif

src/scene/cube.c

@@ -36,9 +36,11 @@ void cubeInit(struct Cube* cube) {
 
 void cubeUpdate(struct Cube* cube) {
     collisionObjectCollideWithPlane(&cube->collisionObject, &gFloorObject, &gContactSolver);
-
     contactSolverSolve(&gContactSolver);
+    rigidBodyUpdate(&cube->rigidBody);
 
+    collisionObjectCollideWithPlane(&cube->collisionObject, &gFloorObject, &gContactSolver);
+    contactSolverSolve(&gContactSolver);
     rigidBodyUpdate(&cube->rigidBody);
 }
 

src/scene/scene.c

@@ -13,6 +13,7 @@
 #include "controls/controller.h"
 #include "shadow_map.h"
 #include "../physics/point_constraint.h"
+#include "../physics/debug_renderer.h"
 #include "../controls/controller.h"
 
 #include "../levels/test_chamber_00_0/header.h"
@@ -82,6 +83,8 @@ void sceneRender(struct Scene* scene, struct RenderState* renderState, struct Gr
     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);
+
+    contactSolverDebugDraw(&gContactSolver, renderState);
 }
 
 unsigned ignoreInputFrames = 10;

src/util/time.h

@@ -6,7 +6,7 @@
 extern float gTimePassed;
 extern OSTime gLastTime;
 
-#define FIXED_DELTA_TIME    (1.0f / 60.0f)
+#define FIXED_DELTA_TIME    (1.0f / 120.0f)
 
 void timeUpdateDelta();