Use persistant contacts

src/main.c

@@ -126,6 +126,7 @@ static void gameProc(void* arg) {
 
     heapInit(_heapStart, memoryEnd);
 
+    contactSolverInit(&gContactSolver);
     sceneInit(&gScene);
     romInit();
     controllersInit();

src/math/mathf.c

@@ -92,6 +92,16 @@ float clampf(float input, float min, float max) {
     return input;
 }
 
+float signf(float input) {
+    if (input > 0.0f) {
+        return 1.0f;
+    } else if (input < 0.0f) {
+        return -1.0f;
+    } else {
+        return 0.0f;
+    }
+}
+
 float minf(float a, float b) {
     return a < b ? a : b;
 }

src/math/mathf.h

@@ -13,6 +13,7 @@ float mathfBounceBackLerp(float t);
 float mathfRandomFloat();
 float mathfMod(float input, float divisor);
 float clampf(float input, float min, float max);
+float signf(float input);
 
 float sqrtf(float in);
 float powf(float base, float exp);

src/math/vector3.c

@@ -8,6 +8,12 @@ struct Vector3 gForward = {0.0f, 0.0f, 1.0f};
 struct Vector3 gZeroVec = {0.0f, 0.0f, 0.0f};
 struct Vector3 gOneVec = {1.0f, 1.0f, 1.0f};
 
+void vector3Abs(struct Vector3* in, struct Vector3* out) {
+    out->x = fabsf(in->x);
+    out->y = fabsf(in->y);
+    out->z = fabsf(in->z);
+}
+
 void vector3Negate(struct Vector3* in, struct Vector3* out) {
     out->x = -in->x;
     out->y = -in->y;

src/math/vector3.h

@@ -18,6 +18,7 @@ extern struct Vector3 gOneVec;
 
 #define VECTOR3_AS_ARRAY(vector) ((float*)(vector))
 
+void vector3Abs(struct Vector3* in, struct Vector3* out);
 void vector3Negate(struct Vector3* in, struct Vector3* out);
 void vector3Scale(struct Vector3* in, struct Vector3* out, float scale);
 void vector3Add(struct Vector3* a, struct Vector3* b, struct Vector3* out);

src/physics/collision.h

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

src/physics/collision_box.c

@@ -4,34 +4,35 @@
 
 #define NORMAL_ZERO_BIAS    0.001f
 
+#define NEGATIVE_PENETRATION_BIAS   0.1f
+
 struct ColliderCallbacks gCollisionBoxCallbacks = {
     collisionBoxCollidePlane,
     collisionBoxSolidMofI,
 };
 
-int _collsionBuildPlaneContact(struct Transform* boxTransform, struct Plane* plane, struct Vector3* point, struct ContactState* contact) {
+void _collsionBuildPlaneContact(struct Transform* boxTransform, struct Plane* plane, struct Vector3* point, struct ContactConstraintState* output, int id) {
-    quatMultVector(&boxTransform->rotation, point, &contact->rb);
-
     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(plane, &worldPoint);
 
-    contact->ra = gZeroVec;
+    if (penetration >= NEGATIVE_PENETRATION_BIAS) {
-
+        return;
-    contact->penetration = planePointDistance(plane, &worldPoint);
-
-    if (contact->penetration >= 0.0f) {
-        return 0;
     }
 
-    contact->normalImpulse = 0.0f;
+
-    contact->tangentImpulse[0] = 0.0f;
+    vector3AddScaled(&worldPoint, &plane->normal, penetration, &contact->ra);
-    contact->tangentImpulse[1] = 0.0f;
+
+    ++output->contactCount;
+    contact->id = id;
+    contact->penetration = penetration;
     contact->bias = 0;
     contact->normalMass = 0;
     contact->tangentMass[0] = 0.0f;
     contact->tangentMass[1] = 0.0f;
-
-    return 1;
 }
 
 
@@ -55,18 +56,15 @@ int collisionBoxCollidePlane(void* data, struct Transform* boxTransform, struct
 
     struct Vector3 deepestCorner;
 
-    int splitAxis[3];
+    int id = 0;
-    int splitAxisCount = 0;
 
     for (int axis = 0; axis < 3; ++axis) {
         float normalValue = VECTOR3_AS_ARRAY(&normalInBoxSpace)[axis];
-        if (normalValue < -NORMAL_ZERO_BIAS) {
+        if (normalValue < 0) {
             VECTOR3_AS_ARRAY(&deepestCorner)[axis] = VECTOR3_AS_ARRAY(&box->sideLength)[axis];
-        } else if (normalValue > NORMAL_ZERO_BIAS) {
-            VECTOR3_AS_ARRAY(&deepestCorner)[axis] = -VECTOR3_AS_ARRAY(&box->sideLength)[axis];
         } else {
-            splitAxis[splitAxisCount] = axis;
+            VECTOR3_AS_ARRAY(&deepestCorner)[axis] = -VECTOR3_AS_ARRAY(&box->sideLength)[axis];
-            ++splitAxisCount;
+            id |= 1 << axis;
         }
     }
 
@@ -79,54 +77,9 @@ int collisionBoxCollidePlane(void* data, struct Transform* boxTransform, struct
     output->restitution = 0.0f;
     output->friction = 1.0f;
 
-    if (splitAxisCount == 0) {
+    _collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, output, id);
-        if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
-            return output->contactCount;
-        }
-        ++output->contactCount;
-    } else if (splitAxisCount == 1) {
-        VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
-        if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
-            return output->contactCount;
-        }
-        ++output->contactCount;
-        VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = -VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
-        if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
-            return output->contactCount;
-        }
-        ++output->contactCount;
-    } else if (splitAxisCount == 2) {
-        VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
-        VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[1]] = VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[1]];
-        if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
-            return output->contactCount;
-        }
-        ++output->contactCount;
 
-        VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = -VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
+    return 1;
-        if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
-            return output->contactCount;
-        }
-        ++output->contactCount;
-
-        VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
-        VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[1]] = -VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[1]];
-        if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
-            return output->contactCount;
-        }
-        ++output->contactCount;
-
-        VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = -VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
-        if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
-            return output->contactCount;
-        }
-        ++output->contactCount;
-    } else {
-        // should not happen
-        return output->contactCount;
-    }
-
-    return output->contactCount;
 }
 
 void collisionBoxCollideQuad(struct CollisionBox* box, struct Transform* boxTransform, struct CollisionQuad* quad) {
@@ -137,7 +90,6 @@ void collisionBoxCollideQuad(struct CollisionBox* box, struct Transform* boxTran
     struct Vector3 normalInBoxSpace;
 
     quatMultVector(&inverseBoxRotation, &quad->normal, &normalInBoxSpace);
-
 }
 
 float collisionBoxSolidMofI(struct ColliderTypeData* typeData, float mass) {

src/physics/collision_edge.h

@@ -0,0 +1,12 @@
+#ifndef __COLLISION_EDGE_H__
+#define __COLLISION_EDGE_H__
+
+#include "../math/vector3.h"
+
+struct CollisionEdge {
+    struct Vector3 endpoint;
+    struct Vector3 direction;
+    float length;
+};
+
+#endif

src/physics/collision_object.c

@@ -0,0 +1,32 @@
+#include "collision_object.h"
+
+void collisionObjectInit(struct CollisionObject* object, struct ColliderTypeData *collider, struct RigidBody* body, float mass) {
+    object->collider = collider;
+    object->body = body;
+    rigidBodyInit(body, mass, collider->callbacks->mofICalculator(collider, mass));
+}
+
+
+void collisionObjectCollideWithPlane(struct CollisionObject* object, struct CollisionObject* plane, struct ContactSolver* contactSolver) {
+    CollideWithPlane planeCollider = object->collider->callbacks->collideWithPlane;
+
+    if (!planeCollider) {
+        return;
+    }
+
+    struct ContactConstraintState localContact;
+    localContact.contactCount = 0;
+
+    struct ContactConstraintState* contact = contactSolverPeekContact(contactSolver, plane, object);
+    
+    if (planeCollider(object->collider->data, &object->body->transform, plane->collider->data, &localContact)) {
+
+        if (!contact) {
+            return;
+        }
+        
+        contactSolverAssign(contact, &localContact);
+    } else if (contact) {
+        contactSolverRemoveContact(contactSolver, contact);
+    }
+}

src/physics/collision_object.h

@@ -0,0 +1,16 @@
+#ifndef __COLLISION_OBJECT_H__
+#define __COLLISION_OBJECT_H__
+
+#include "rigid_body.h"
+#include "collision.h"
+
+struct CollisionObject {
+    struct ColliderTypeData *collider;
+    struct RigidBody* body;
+};
+
+void collisionObjectInit(struct CollisionObject* object, struct ColliderTypeData *collider, struct RigidBody* body, float mass);
+
+void collisionObjectCollideWithPlane(struct CollisionObject* object, struct CollisionObject* plane, struct ContactSolver* contactSolver);
+
+#endif

src/physics/collision_point.c

@@ -0,0 +1,44 @@
+#include "collision_point.h"
+
+#include "contact_solver.h"
+#include "collision_box.h"
+#include "../math/transform.h"
+#include "../math/mathf.h"
+
+int collisionBoxCollidePoint(void* data, struct Transform* boxTransform, struct Vector3* point, struct ContactConstraintState* output) {
+    struct Vector3 localSpace;
+    transformPointInverse(boxTransform, point, &localSpace);
+
+    struct Vector3 absLocalspace;
+    vector3Abs(&localSpace, &absLocalspace);
+    
+    if (absLocalspace.x > 0.0f || absLocalspace.y > 0.0f || absLocalspace.z > 0.0f) {
+        return 0;
+    }
+
+    struct CollisionBox* box = (struct CollisionBox*)data;
+
+    float axisDistance = box->sideLength.x - absLocalspace.x;
+    int axis = 0;
+
+    float check = box->sideLength.y - absLocalspace.y;
+
+    if (check < axisDistance) {
+        axisDistance = check;
+        axis = 1;
+    }
+
+    check = box->sideLength.z - absLocalspace.z;
+
+    if (check < axisDistance) {
+        axisDistance = check;
+        axis = 2;
+    }
+
+
+    output->normal = gZeroVec;
+    VECTOR3_AS_ARRAY(&output->normal)[axis] = signf(VECTOR3_AS_ARRAY(&localSpace)[0]);
+    output->contactCount = 1;
+    
+    return 1;
+}

src/physics/collision_point.h

@@ -0,0 +1,10 @@
+#ifndef __COLLISION_POINT_H__
+#define __COLLISION_POINT_H__
+
+#include "../math/vector3.h"
+
+struct CollisionPoint {
+    struct Vector3 worldPoint;
+};
+
+#endif

src/physics/collision_quad.h

@@ -6,7 +6,9 @@
 struct CollisionQuad {
     struct Vector3 corner;
     struct Vector3 edgeA;
+    float edgeALength;
     struct Vector3 edgeB;
+    float edgeBLength;
     struct Vector3 normal;
 };
 

src/physics/contact_solver.c

@@ -3,28 +3,48 @@
 #include "../util/time.h"
 #include "../math/mathf.h"
 #include "rigid_body.h"
+#include "collision_object.h"
+
+#include <string.h>
+
+struct ContactSolver gContactSolver;
+
+void contactSolverInit(struct ContactSolver* contactSolver) {
+	memset(contactSolver, 0, sizeof(struct ContactSolver));
+
+	contactSolver->contactCapacity = MAX_CONTACT_COUNT;
+
+	contactSolver->unusedContacts = &contactSolver->contacts[0];
+
+	for (int i = 1; i < MAX_CONTACT_COUNT; ++i) {
+		contactSolver->contacts[i-1].next = &contactSolver->contacts[i];
+	}
+}
 
 void contactSolverPreSolve(struct ContactSolver* contactSolver) {
-	for ( int i = 0; i < contactSolver->contactCount; ++i )
+	struct ContactConstraintState *cs = contactSolver->activeContacts;
+	
+    while (cs)
 	{
-		struct ContactConstraintState *cs = contactSolver->contacts + i;
-
 		struct Vector3* vA;
 		struct Vector3* wA;
 		struct Vector3* vB;
 		struct Vector3* wB;
 
-		if (cs->bodyA) {
+		struct RigidBody* bodyA = cs->shapeA->body;
-			vA = &cs->bodyA->velocity;
+		struct RigidBody* bodyB = cs->shapeB->body;
-			wA = &cs->bodyA->angularVelocity;
+
+		if (bodyA) {
+			vA = &bodyA->velocity;
+			wA = &bodyA->angularVelocity;
 		} else {
 			vA = NULL;
 			wA = NULL;
 		}
 
-		if (cs->bodyB) {
+		if (bodyB) {
-			vB = &cs->bodyB->velocity;
+			vB = &bodyB->velocity;
-			wB = &cs->bodyB->angularVelocity;
+			wB = &bodyB->angularVelocity;
 		} else {
 			vB = NULL;
 			wB = NULL;
@@ -41,24 +61,24 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
             vector3Cross(&c->rb, &cs->normal, &rbCn);
 			float nm = 0;
 			
-			if (cs->bodyA) {
+			if (bodyA) {
-				nm += cs->bodyA->massInv;
+				nm += bodyA->massInv;
 			}
 
-			if (cs->bodyB) {
+			if (bodyB) {
-				nm += cs->bodyB->massInv;
+				nm += bodyB->massInv;
 			}
 
 			float tm[ 2 ];
 			tm[ 0 ] = nm;
 			tm[ 1 ] = nm;
 
-			if (cs->bodyA) {
+			if (bodyA) {
-				nm += cs->bodyA->momentOfInertiaInv * vector3MagSqrd(&raCn);
+				nm += bodyA->momentOfInertiaInv * vector3MagSqrd(&raCn);
 			}
 
-			if (cs->bodyB) {
+			if (bodyB) {
-				nm += cs->bodyB->momentOfInertiaInv * vector3MagSqrd(&rbCn);
+				nm += bodyB->momentOfInertiaInv * vector3MagSqrd(&rbCn);
 			}
 
 			c->normalMass = safeInvert( nm );
@@ -70,11 +90,11 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 				struct Vector3 rbCt;
                 vector3Cross(&cs->tangentVectors[ i ], &c->rb, &rbCt);
 
-				if (cs->bodyA) {
+				if (bodyA) {
-					tm[ i ] += cs->bodyA->momentOfInertiaInv * vector3MagSqrd(&raCt);
+					tm[ i ] += bodyA->momentOfInertiaInv * vector3MagSqrd(&raCt);
 				}
-				if (cs->bodyB) {
+				if (bodyB) {
-					tm[ i ] += cs->bodyB->momentOfInertiaInv * vector3MagSqrd(&rbCt);
+					tm[ i ] += bodyB->momentOfInertiaInv * vector3MagSqrd(&rbCt);
 				}
 
 				c->tangentMass[ i ] = safeInvert( tm[ i ] );
@@ -82,7 +102,7 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 
 			// Precalculate bias factor
 			c->bias = -Q3_BAUMGARTE * (1.0f / FIXED_DELTA_TIME) * minf(0.0f, c->penetration + Q3_PENETRATION_SLOP);
-
+			
 			// Warm start contact
 			struct Vector3 P;
             vector3Scale(&cs->normal, &P, c->normalImpulse);
@@ -95,16 +115,16 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 
             struct Vector3 w;
 
-			if (cs->bodyA) {
+			if (bodyA) {
-				vector3AddScaled(vA, &P, -cs->bodyA->massInv, vA);
+				vector3AddScaled(vA, &P, -bodyA->massInv, vA);
 				vector3Cross(&c->ra, &P, &w);
-				vector3AddScaled(wA, &w, -cs->bodyA->momentOfInertiaInv, wA);
+				vector3AddScaled(wA, &w, -bodyA->momentOfInertiaInv, wA);
 			}
 
-			if (cs->bodyB) {
+			if (bodyB) {
-				vector3AddScaled(vB, &P, cs->bodyB->massInv, vB);
+				vector3AddScaled(vB, &P, bodyB->massInv, vB);
 				vector3Cross(&c->rb, &P, &w);
-				vector3AddScaled(wB, &w, cs->bodyB->momentOfInertiaInv, wB);
+				vector3AddScaled(wB, &w, bodyB->momentOfInertiaInv, wB);
 			}
 
             struct Vector3 velocity;
@@ -128,30 +148,35 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 			if ( dv < -1.0f )
 				c->bias += -(cs->restitution) * dv;
 		}
+
+		cs = cs->next;
 	}
 }
 
 void contactSolverIterate(struct ContactSolver* contactSolver) {
-    for ( int i = 0; i < contactSolver->contactCount; ++i )
+	struct ContactConstraintState *cs = contactSolver->activeContacts;
-	{
-		struct ContactConstraintState *cs = contactSolver->contacts + i;
 
+    while (cs)
+	{
 		struct Vector3* vA;
 		struct Vector3* wA;
 		struct Vector3* vB;
 		struct Vector3* wB;
 
-		if (cs->bodyA) {
+		struct RigidBody* bodyA = cs->shapeA->body;
-			vA = &cs->bodyA->velocity;
+		struct RigidBody* bodyB = cs->shapeB->body;
-			wA = &cs->bodyA->angularVelocity;
+
+		if (bodyA) {
+			vA = &bodyA->velocity;
+			wA = &bodyA->angularVelocity;
 		} else {
 			vA = NULL;
 			wA = NULL;
 		}
 
-		if (cs->bodyB) {
+		if (bodyB) {
-			vB = &cs->bodyB->velocity;
+			vB = &bodyB->velocity;
-			wB = &cs->bodyB->angularVelocity;
+			wB = &bodyB->angularVelocity;
 		} else {
 			vB = NULL;
 			wB = NULL;
@@ -199,15 +224,15 @@ void contactSolverIterate(struct ContactSolver* contactSolver) {
 
                     struct Vector3 w;
 					if (vA) {
-						vector3AddScaled(vA, &impulse, -cs->bodyA->massInv, vA);
+						vector3AddScaled(vA, &impulse, -bodyA->massInv, vA);
 						vector3Cross(&c->ra, &impulse, &w);
-						vector3AddScaled(wA, &w, -cs->bodyA->momentOfInertiaInv, wA);
+						vector3AddScaled(wA, &w, -bodyA->momentOfInertiaInv, wA);
 					}
 
 					if (vB) {
-						vector3AddScaled(vB, &impulse, cs->bodyB->massInv, vB);
+						vector3AddScaled(vB, &impulse, bodyB->massInv, vB);
 						vector3Cross(&c->rb, &impulse, &w);
-						vector3AddScaled(wB, &w, cs->bodyB->momentOfInertiaInv, wB);
+						vector3AddScaled(wB, &w, bodyB->momentOfInertiaInv, wB);
 					}
 				}
 			}
@@ -244,18 +269,20 @@ void contactSolverIterate(struct ContactSolver* contactSolver) {
 
                 struct Vector3 w;
 				if (vA) {
-					vector3AddScaled(vA, &impulse, -cs->bodyA->massInv, vA);
+					vector3AddScaled(vA, &impulse, -bodyA->massInv, vA);
 					vector3Cross(&c->ra, &impulse, &w);
-					vector3AddScaled(wA, &w, -cs->bodyA->momentOfInertiaInv, wA);
+					vector3AddScaled(wA, &w, -bodyA->momentOfInertiaInv, wA);
 				}
 
 				if (vB) {
-					vector3AddScaled(vB, &impulse, cs->bodyB->massInv, vB);
+					vector3AddScaled(vB, &impulse, bodyB->massInv, vB);
 					vector3Cross(&c->rb, &impulse, &w);
-					vector3AddScaled(wB, &w, cs->bodyB->momentOfInertiaInv, wB);
+					vector3AddScaled(wB, &w, bodyB->momentOfInertiaInv, wB);
 				}
 			}
 		}
+
+		cs = cs->next;
     }
 }
 
@@ -265,4 +292,113 @@ void contactSolverSolve(struct ContactSolver* solver) {
 	contactSolverIterate(solver);
 	contactSolverIterate(solver);
 	// contactSolverIterate(solver);
+}
+
+struct ContactConstraintState* contactSolverPeekContact(struct ContactSolver* solver, struct CollisionObject* shapeA, struct CollisionObject* shapeB) {
+	struct ContactConstraintState* curr = solver->activeContacts;
+
+	while (curr) {
+		if ((curr->shapeA == shapeA && curr->shapeB == shapeB) || (curr->shapeA == shapeB && curr->shapeB == shapeA)) {
+			return curr;
+		}
+
+		curr = curr->next;
+	}
+
+	struct ContactConstraintState* result = solver->unusedContacts;
+
+	if (result) {
+		result->shapeA = shapeA;
+		result->shapeB = shapeB;
+		result->contactCount = 0;
+
+		solver->unusedContacts = result->next;
+		result->next = solver->activeContacts;
+		solver->activeContacts = result;
+	}
+
+	return result;
+}
+
+void contactSolverRemoveContact(struct ContactSolver* solver, struct ContactConstraintState* toRemove) {
+	struct ContactConstraintState* curr = solver->activeContacts;
+	struct ContactConstraintState* prev = NULL;
+
+	while (curr) {
+		if (curr == toRemove) {
+			break;
+		}
+
+		prev = curr;
+		curr = curr->next;
+	}
+
+	if (!curr) {
+		return;
+	}
+
+	if (prev) {
+		prev->next = curr->next;
+	} else {
+		solver->activeContacts = curr->next;
+	}
+
+	curr->next = solver->unusedContacts;
+	solver->unusedContacts = curr;
+}
+
+struct ContactState* contactSolverGetContact(struct ContactConstraintState* contact, int id) {
+	int i;
+
+	for (i = 0; i < contact->contactCount; ++i) {
+		if (contact->contacts[i].id == id) {
+			return &contact->contacts[i];
+		}
+	}
+
+	if (i == MAX_CONTACTS_PER_MANIFOLD) {
+		return NULL;
+	}
+
+	struct ContactState* result = &contact->contacts[i];
+
+	result->normalImpulse = 0.0f;
+	result->tangentImpulse[0] = 0.0f;
+	result->tangentImpulse[1] = 0.0f;
+	result->id = id;
+
+	++contact->contactCount;
+	
+	return result;
+}
+
+void contactSolverAssign(struct ContactConstraintState* into, struct ContactConstraintState* from) {
+	for (int sourceIndex = 0; sourceIndex < from->contactCount; ++sourceIndex) {
+		int targetIndex;
+
+		struct ContactState* sourceContact = &from->contacts[sourceIndex];
+
+		for (targetIndex = 0; targetIndex < into->contactCount; ++targetIndex) {
+			struct ContactState* targetContact = &into->contacts[targetIndex];
+
+			if (sourceContact->id == targetContact->id) {
+				sourceContact->normalImpulse = targetContact->normalImpulse;
+				// TODO reproject tangents
+				sourceContact->tangentImpulse[0] = targetContact->tangentImpulse[0];
+				sourceContact->tangentImpulse[1] = targetContact->tangentImpulse[1];
+				break;
+			}
+		}
+	}
+
+	for (int sourceIndex = 0; sourceIndex < from->contactCount; ++sourceIndex) {
+		into->contacts[sourceIndex] = from->contacts[sourceIndex];
+	}
+
+	into->contactCount = from->contactCount;
+	into->tangentVectors[0] = from->tangentVectors[0];
+	into->tangentVectors[1] = from->tangentVectors[1];
+	into->normal = from->normal;
+	into->restitution = from->restitution;
+	into->friction = from->friction;
 }

src/physics/contact_solver.h

@@ -3,11 +3,11 @@
 
 #include "../math/vector3.h"
 
-struct RigidBody;
+struct CollisionObject;
 
 #define Q3_BAUMGARTE 0.2f
 
-#define Q3_PENETRATION_SLOP 0.05f
+#define Q3_PENETRATION_SLOP 0.01f
 
 #define ENABLE_FRICTION 1
 
@@ -19,6 +19,7 @@ struct VelocityState
 
 struct ContactState
 {
+	int id;
 	struct Vector3 ra;					// Vector from C.O.M to contact position
 	struct Vector3 rb;					// Vector from C.O.M to contact position
 	float penetration;			// Depth of penetration from collision
@@ -29,26 +30,43 @@ struct ContactState
 	float tangentMass[ 2 ];		// Tangent constraint mass
 };
 
+#define MAX_CONTACTS_PER_MANIFOLD	8
+
 struct ContactConstraintState
 {
-	struct ContactState contacts[ 8 ];
+	struct ContactState contacts[ MAX_CONTACTS_PER_MANIFOLD ];
 	int contactCount;
 	struct Vector3 tangentVectors[ 2 ];	// Tangent vectors
 	struct Vector3 normal;				// From A to B
 	float restitution;
 	float friction;
-	struct RigidBody* bodyA;
+	struct CollisionObject* shapeA;
-	struct RigidBody* bodyB;
+	struct CollisionObject* shapeB;
+	struct ContactConstraintState* next;
 };
 
 #define MAX_CONTACT_COUNT	8
 
 struct ContactSolver {
     struct ContactConstraintState contacts[MAX_CONTACT_COUNT];
-    int contactCount;
+	struct ContactConstraintState* unusedContacts;
+	struct ContactConstraintState* activeContacts;
 	int contactCapacity;
 };
 
+extern struct ContactSolver gContactSolver;
+
+void contactSolverInit(struct ContactSolver* contactSolver);
+
 void contactSolverSolve(struct ContactSolver* solver);
 
-#endif
+struct ContactConstraintState* contactSolverPeekContact(struct ContactSolver* solver, struct CollisionObject* shapeA, struct CollisionObject* shapeB);
+
+void contactSolverRemoveContact(struct ContactSolver* solver, struct ContactConstraintState* toRemove);
+
+struct ContactState* contactSolverGetContact(struct ContactConstraintState* contact, int id);
+
+void contactSolverAssign(struct ContactConstraintState* into, struct ContactConstraintState* from);
+
+#endif
+

src/physics/rigid_body.c

@@ -5,9 +5,7 @@
 #include "defs.h"
 #include <math.h>
 
-void rigidBodyInit(struct RigidBody* rigidBody, struct ColliderTypeData* collider, float mass) {
+void rigidBodyInit(struct RigidBody* rigidBody, float mass, float momentOfIniteria) {
-    rigidBody->collider = collider;
-
     transformInitIdentity(&rigidBody->transform);
     rigidBody->velocity = gZeroVec;
     rigidBody->angularVelocity = gZeroVec;
@@ -15,7 +13,7 @@ void rigidBodyInit(struct RigidBody* rigidBody, struct ColliderTypeData* collide
     rigidBody->mass = mass;
     rigidBody->massInv = 1.0f / mass;
 
-    rigidBody->momentOfInertia = collider->callbacks->mofICalculator(collider, mass);
+    rigidBody->momentOfInertia = momentOfIniteria;
     rigidBody->momentOfInertiaInv = 1.0f / rigidBody->momentOfInertia;
 }
 
@@ -58,119 +56,4 @@ float rigidBodyMassInverseAtLocalPoint(struct RigidBody* rigidBody, struct Vecto
     struct Vector3 crossPoint;
     vector3Cross(localPoint, normal, &crossPoint);
     return rigidBody->massInv + rigidBody->momentOfInertiaInv * vector3MagSqrd(&crossPoint);
-}
-
-void rigidBodyResolveContact(struct RigidBody* bodyA, struct RigidBody* bodyB, struct ContactPoint* contactPoint) {
-    struct Vector3 aVelocity;
-    struct Vector3 bVelocity;
-
-    struct Vector3 localPositionA;
-    struct Vector3 localPositionB;
-
-    if (bodyA) {
-        vector3Sub(&contactPoint->point, &bodyA->transform.position, &localPositionA);
-        rigidBodyVelocityAtLocalPoint(bodyA, &localPositionA, &aVelocity);
-    } else {
-        aVelocity = gZeroVec;
-    }
-
-    if (bodyB) {
-        vector3Sub(&contactPoint->point, &bodyB->transform.position, &localPositionB);
-        rigidBodyVelocityAtLocalPoint(bodyB, &localPositionB, &bVelocity);
-    } else {
-        bVelocity = gZeroVec;
-    }
-
-    struct Vector3 relativeVelocity;
-    vector3Sub(&aVelocity, &bVelocity, &relativeVelocity);
-
-    float massInverse = 0.0f;
-    float frictionMassInverse = 0.0f;
-    float bounce = 1.0f;
-    float friction = 0.0f;
-
-    float normalVelocity = vector3Dot(&relativeVelocity, &contactPoint->normal);
-
-    struct Vector3 tangentVelocity;
-    vector3AddScaled(&relativeVelocity, &contactPoint->normal, -normalVelocity, &tangentVelocity);
-
-    struct Vector3 tangentDirection;
-    vector3Normalize(&tangentVelocity, &tangentDirection);
-
-    if (bodyA) {
-        massInverse += rigidBodyMassInverseAtLocalPoint(bodyA, &localPositionA, &contactPoint->normal);
-        frictionMassInverse += rigidBodyMassInverseAtLocalPoint(bodyA, &localPositionA, &tangentDirection);
-        bounce = bodyA->collider->bounce;
-        friction = bodyA->collider->friction;
-    }
-
-    if (bodyB) {
-        massInverse += rigidBodyMassInverseAtLocalPoint(bodyB, &localPositionA, &contactPoint->normal);
-        frictionMassInverse += rigidBodyMassInverseAtLocalPoint(bodyB, &localPositionB, &tangentDirection);
-        bounce = MIN(bounce, bodyB->collider->bounce);
-        friction = MAX(friction, bodyB->collider->friction);
-    }
-
-    if (massInverse == 0.0f) {
-        // two immovable objects
-        return;
-    }
-
-    struct Vector3 tangentImpulse;
-
-    float tangentSpeed = sqrt(vector3MagSqrd(&tangentVelocity));
-
-    massInverse = 1.0f / massInverse;
-    frictionMassInverse = 1.0f / frictionMassInverse;
-
-    float impulse = -(1.0f + bounce) * normalVelocity * massInverse;
-    impulse += contactPoint->intersectionDepth * (0.5f * FIXED_DELTA_TIME);
-
-    float frictionImpulse = impulse * friction;
-    float tangentImpulseMag = tangentSpeed * frictionMassInverse;
-
-    if (tangentImpulseMag > frictionImpulse) {
-        tangentImpulseMag = frictionImpulse;
-    }
-
-    if (tangentImpulseMag > 0.00001f) {
-        vector3Scale(&tangentDirection, &tangentImpulse, -tangentImpulseMag);
-    }
-
-    struct Vector3 impulseVector;
-    vector3AddScaled(&tangentImpulse, &contactPoint->normal, impulse, &impulseVector);
-    if (bodyA) {
-        rigidBodyAppyImpulse(bodyA, &contactPoint->point, &impulseVector);
-    }
-
-    if (bodyB) {
-        vector3Scale(&impulseVector, &impulseVector, -impulse);
-        rigidBodyAppyImpulse(bodyB, &contactPoint->point, &impulseVector);
-    }
-}
-
-void rigidBodyCollideWithPlane(struct RigidBody* rigidBody, struct Plane* plane, struct ContactSolver* contactSolver) {
-    CollideWithPlane planeCollider = rigidBody->collider->callbacks->collideWithPlane;
-
-    if (!planeCollider) {
-        return;
-    }
-
-    struct ContactConstraintState* contact = &contactSolver->contacts[contactSolver->contactCount];
-
-    // int prevCount = contact->contactCount;
-
-    int collisionCount = planeCollider(rigidBody->collider->data, &rigidBody->transform, plane, contact);
-
-    if (collisionCount) {
-        contact->bodyB = rigidBody;
-        // if (contact->bodyB != rigidBody || contact->contactCount != prevCount) {
-
-        //     for (int i = 0; i < contact->contactCount; ++i) {
-        //     }
-        // }
-        ++contactSolver->contactCount;
-    } else {
-        contact->bodyB = NULL;
-    }
 }

src/physics/rigid_body.h

@@ -5,8 +5,6 @@
 #include "collision.h"
 
 struct RigidBody {
-    struct ColliderTypeData* collider;
-
     struct Transform transform;
     struct Vector3 velocity;
     struct Vector3 angularVelocity;
@@ -22,14 +20,11 @@ struct RigidBody {
     float momentOfInertiaInv;
 };
 
-void rigidBodyInit(struct RigidBody* rigidBody, struct ColliderTypeData* collider, float mass);
+void rigidBodyInit(struct RigidBody* rigidBody, float mass, float momentOfIniteria);
 void rigidBodyAppyImpulse(struct RigidBody* rigidBody, struct Vector3* worldPoint, struct Vector3* impulse);
 void rigidBodyUpdate(struct RigidBody* rigidBody);
 void rigidBodyVelocityAtLocalPoint(struct RigidBody* rigidBody, struct Vector3* localPoint, struct Vector3* worldVelocity);
 void rigidBodyVelocityAtWorldPoint(struct RigidBody* rigidBody, struct Vector3* worldPoint, struct Vector3* worldVelocity);
 
-void rigidBodyResolveContact(struct RigidBody* bodyA, struct RigidBody* bodyB, struct ContactPoint* contactPoint);
-
-void rigidBodyCollideWithPlane(struct RigidBody* rigidBody, struct Plane* plane, struct ContactSolver* contactSolver);
 
 #endif

src/player/player.c

@@ -16,10 +16,13 @@ void playerInit(struct Player* player) {
     player->pitch = 0.0f;
 }
 
-#define PLAYER_SPEED    (2.0f)
+#define PLAYER_SPEED    (5.0f)
+#define PLAYER_ACCEL    (40.0f)
+#define PLAYER_STOP_ACCEL    (80.0f)
 
 #define ROTATE_RATE     (M_PI * 2.0f)
 #define ROTATE_RATE_DELTA     (M_PI * 0.25f)
+#define ROTATE_RATE_STOP_DELTA (M_PI * 0.25f)
 
 void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
     struct Vector3 forward;
@@ -36,8 +39,18 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
 
     OSContPad* controllerInput = controllersGetControllerData(0);
 
-    vector3AddScaled(&player->transform.position, &forward, -controllerInput->stick_y * FIXED_DELTA_TIME * PLAYER_SPEED / 80.0f, &player->transform.position);
+    struct Vector3 targetVelocity;
-    vector3AddScaled(&player->transform.position, &right, controllerInput->stick_x * FIXED_DELTA_TIME * PLAYER_SPEED / 80.0f, &player->transform.position);
+
+    vector3Scale(&forward, &targetVelocity, -controllerInput->stick_y * PLAYER_SPEED / 80.0f);
+    vector3AddScaled(&targetVelocity, &right, controllerInput->stick_x * PLAYER_SPEED / 80.0f, &targetVelocity);
+
+    vector3MoveTowards(
+        &player->velocity, 
+        &targetVelocity, 
+        vector3Dot(&player->velocity, &targetVelocity) > 0.0f ? PLAYER_ACCEL * FIXED_DELTA_TIME : PLAYER_STOP_ACCEL * FIXED_DELTA_TIME, 
+        &player->velocity
+    );
+    vector3AddScaled(&player->transform.position, &player->velocity, FIXED_DELTA_TIME, &player->transform.position);
 
     float targetYaw = 0.0f;
     float targetPitch = 0.0f;
@@ -49,13 +62,21 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
     }
 
     if (controllerGetButton(0, U_CBUTTONS)) {
-        targetPitch -= ROTATE_RATE;
-    } else if (controllerGetButton(0, D_CBUTTONS)) {
         targetPitch += ROTATE_RATE;
+    } else if (controllerGetButton(0, D_CBUTTONS)) {
+        targetPitch -= ROTATE_RATE;
     }
 
-    player->yawVelocity = mathfMoveTowards(player->yawVelocity, targetYaw, ROTATE_RATE_DELTA);
+    player->yawVelocity = mathfMoveTowards(
-    player->pitchVelocity = mathfMoveTowards(player->pitchVelocity, targetPitch, ROTATE_RATE_DELTA);
+        player->yawVelocity, 
+        targetYaw, 
+        player->yawVelocity * targetYaw > 0.0f ? ROTATE_RATE_DELTA : ROTATE_RATE_STOP_DELTA
+    );
+    player->pitchVelocity = mathfMoveTowards(
+        player->pitchVelocity, 
+        targetPitch, 
+        player->pitchVelocity * targetPitch > 0.0f ? ROTATE_RATE_DELTA : ROTATE_RATE_STOP_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);
@@ -71,7 +92,7 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
     if (player->grabbing) {
         struct Vector3 grabPoint;
         transformPoint(cameraTransform, &gGrabDistance, &grabPoint);
-        pointConstraintMoveToPoint(player->grabbing, &grabPoint, 5.0f);
+        pointConstraintMoveToPoint(player->grabbing, &grabPoint, 20.0f);
         pointConstraintRotateTo(player->grabbing, &cameraTransform->rotation, 5.0f);
     }
 }

src/player/player.h

@@ -8,6 +8,7 @@
 struct Player {
     struct Transform transform;
     struct RigidBody* grabbing;
+    struct Vector3 velocity;
     float pitch;
     float pitchVelocity;
     float yaw;

src/scene/cube.c

@@ -9,7 +9,18 @@ struct CollisionBox gCubeCollisionBox = {
 
 struct Plane gFloor = {{0.0f, 1.0f, 0.0f}, 0.0f};
 
-struct ContactSolver gContactSolver;
+struct ColliderTypeData gFloorColliderType = {
+    CollisionShapeTypeQuad,
+    &gFloor,
+    0.0f,
+    1.0f,
+    NULL,
+};
+
+struct CollisionObject gFloorObject = {
+    &gFloorColliderType,
+    NULL,
+};
 
 struct ColliderTypeData gCubeCollider = {
     CollisionShapeTypeBox,
@@ -20,13 +31,11 @@ struct ColliderTypeData gCubeCollider = {
 };
 
 void cubeInit(struct Cube* cube) {
-    rigidBodyInit(&cube->rigidBody, &gCubeCollider, 1.0f);
+    collisionObjectInit(&cube->collisionObject, &gCubeCollider, &cube->rigidBody, 1.0f);
 }
 
 void cubeUpdate(struct Cube* cube) {
-    gContactSolver.contactCount = 0;
+    collisionObjectCollideWithPlane(&cube->collisionObject, &gFloorObject, &gContactSolver);
-
-    rigidBodyCollideWithPlane(&cube->rigidBody, &gFloor, &gContactSolver);
 
     contactSolverSolve(&gContactSolver);
 

src/scene/cube.h

@@ -3,8 +3,10 @@
 
 #include "../physics/rigid_body.h"
 #include "../graphics/renderstate.h"
+#include "../physics/collision_object.h"
 
 struct Cube {
+    struct CollisionObject collisionObject;
     struct RigidBody rigidBody;
 };
 

src/scene/scene.c

@@ -24,7 +24,7 @@ void sceneInit(struct Scene* scene) {
     playerInit(&scene->player);
 
     scene->player.transform.position = gStartPosition;
-    quatAxisAngle(&gUp, M_PI, &scene->player.transform.rotation);
+    scene->player.yaw = M_PI;
 
     portalInit(&scene->portals[0], 0);
     portalInit(&scene->portals[1], PortalFlagsOddParity);
@@ -61,6 +61,8 @@ void sceneRenderWithProperties(void* data, struct RenderProps* properties, struc
     cubeRender(&scene->cube, renderState);
 }
 
+#define SOLID_COLOR        0, 0, 0, ENVIRONMENT, 0, 0, 0, ENVIRONMENT
+
 void sceneRender(struct Scene* scene, struct RenderState* renderState, struct GraphicsTask* task) {
     struct RenderProps renderProperties;
 
@@ -70,11 +72,24 @@ void sceneRender(struct Scene* scene, struct RenderState* renderState, struct Gr
     renderProperties.currentDepth = STARTING_RENDER_DEPTH;
 
     sceneRenderWithProperties(scene, &renderProperties, renderState);
+
+    gDPPipeSync(renderState->dl++);
+    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);
+    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);
 }
 
 unsigned ignoreInputFrames = 10;
 
 void sceneUpdate(struct Scene* scene) {
+    OSTime frameStart = osGetTime();
+    scene->lastFrameTime = frameStart - scene->lastFrameStart;
+
     playerUpdate(&scene->player, &scene->camera.transform);
     cubeUpdate(&scene->cube);
 
@@ -85,4 +100,7 @@ void sceneUpdate(struct Scene* scene) {
             scene->player.grabbing = &scene->cube.rigidBody;
         }
     }
+
+    scene->cpuTime = osGetTime() - frameStart;
+    scene->lastFrameStart = frameStart;
 }

src/scene/scene.h

@@ -15,6 +15,9 @@ struct Scene {
     struct Player player;
     struct Portal portals[2];
     struct Cube cube;
+    OSTime cpuTime;
+    OSTime lastFrameStart;
+    OSTime lastFrameTime;
 };
 
 struct GraphicsTask;

src/util/linked_list.c

@@ -0,0 +1 @@
+#include "linked_list.h"

src/util/linked_list.h

@@ -0,0 +1,9 @@
+#ifndef __LINKED_LIST_H__
+#define __LINKED_LIST_H__
+
+struct LinkedListNode {
+    struct LinkedListNode* next;
+    struct LinkedListNode* prev;
+};
+
+#endif