Rectangle intersection with a line complete

2 files changed, 111 insertions, 1 deletions

diff --git a/frontend/2d/_collisions/rectangle_line/dist/output.wasm b/frontend/2d/_collisions/rectangle_line/dist/output.wasm
index 11fa4c5..41e549e 100755
--- a/frontend/2d/_collisions/rectangle_line/dist/output.wasm
+++ b/frontend/2d/_collisions/rectangle_line/dist/output.wasm
diff --git a/frontend/2d/_collisions/rectangle_line/main.cpp b/frontend/2d/_collisions/rectangle_line/main.cpp
index 321e8b5..39cb079 100644
--- a/frontend/2d/_collisions/rectangle_line/main.cpp
+++ b/frontend/2d/_collisions/rectangle_line/main.cpp
@@ -51,6 +51,14 @@ struct Rigidbody {
     }
 };
 
+struct IntersectionResult {
+    bool intersect = false;
+    Vector2 collisionNormal;
+    Vector2 relativeVelocity;
+    Vector2 firstPointOfApplication;
+    Vector2 secondPointOfApplication;
+};
+
 struct Rectangle {
 	OrthographicShape shape;
 	Rigidbody body;
@@ -79,6 +87,8 @@ struct Rectangle {
 		}
 
 		shape.load(vertices, 6, renderer);
+		body.reset();
+		body.momentOfInertia = (1.f / 12.f) * body.mass * (width + height * height * height);
 	}
 
 	void update(float32 dtSeconds) {
@@ -172,15 +182,115 @@ void load() {
     segmentList[3].load(&renderer, Vector4().fromColor(205, 178, 214, 255.f), Vector2 { 50.f, 150.f }, Vector2 { context.width - 50.f, 50.f });
 
 	rectangle.load(&renderer, Vector4 { 230.f, 182.f, 35.f, 255.f }, 64.f, 32.f);
-	rectangle.body.position = Vector2 { context.width / 2.f, context.height / 2.f };
+	rectangle.body.position = Vector2 { context.width / 3.f, context.height / 2.f };
 
     mainLoop.run(update);
 }
 
+float32 getDistancePointToLine(Vector2 point, LineSegment* segment) {
+	float32 xLength = segment->end.x - segment->start.x;
+	float32 yLength = segment->end.y - segment->start.y;
+	return fabs(xLength * (segment->start.y - point.y) - (segment->start.x - point.x) * yLength)
+		/ sqrtf((xLength * xLength) + (yLength * yLength));
+}
+
+const float32 EPSILON = 1.f;
+IntersectionResult getIntersection(Rectangle* rectangle, LineSegment* segment) {
+	IntersectionResult ir;
+	
+	float32 halfWidth = rectangle->width / 2.f;
+	float32 halfHeight = rectangle->height / 2.f;
+	
+	Vector2 bottomLeft = rectangle->shape.model * Vector2 { -halfWidth, -halfHeight };
+	Vector2 topLeft = rectangle->shape.model * Vector2 { -halfWidth, halfHeight };
+	Vector2 bottomRight = rectangle->shape.model * Vector2 { halfWidth, -halfHeight };
+	Vector2 topRight = rectangle->shape.model * Vector2 { halfWidth, halfHeight };
+
+	Vector2 collisionPoint;
+
+	if (getDistancePointToLine(bottomLeft, segment) <= EPSILON) {
+		ir.intersect = true;
+		collisionPoint = bottomLeft;
+	} else if (getDistancePointToLine(topLeft, segment) <= EPSILON) {
+		ir.intersect = true;
+		collisionPoint = topLeft;
+	} else if (getDistancePointToLine(bottomRight, segment) <= EPSILON) {
+		ir.intersect = true;
+		collisionPoint = bottomRight;
+	} else if (getDistancePointToLine(topRight, segment) <= EPSILON) {
+		ir.intersect = true;
+		collisionPoint = topRight;
+	} else {
+		ir.intersect = false;
+		return ir;
+	}
+
+	ir.collisionNormal = segment->getNormal();
+	ir.relativeVelocity = rectangle->body.velocity - segment->body.velocity;
+	ir.firstPointOfApplication = (collisionPoint - rectangle->body.position);
+	ir.secondPointOfApplication = collisionPoint - segment->getPointOnLine(0.5f);
+
+	return ir;
+}
+
+void resolveCollision(Rigidbody* first, Rigidbody* second, IntersectionResult* ir) {
+    Vector2 relativeVelocity = ir->relativeVelocity;
+    Vector2 collisionNormal  = ir->collisionNormal;
+    Vector2 firstPerp        = ir->firstPointOfApplication.getPerp();
+    Vector2 secondPerp       = ir->secondPointOfApplication.getPerp();
+	float32 firstPerpNorm    = firstPerp.dot(collisionNormal);
+	float32 sndPerpNorm      = secondPerp.dot(collisionNormal);
+
+    float32 cofOfRestition = (first->cofOfRestition + second->cofOfRestition) / 2.f;
+    float32 numerator = (relativeVelocity * (-1 * (1.f + cofOfRestition))).dot(collisionNormal);
+    float32 linearDenomPart = collisionNormal.dot(collisionNormal * (1.f / first->mass + 1.f / second->mass));
+	float32 rotationalDenomPart = (firstPerpNorm * firstPerpNorm) / first->momentOfInertia + (sndPerpNorm * sndPerpNorm) / second->momentOfInertia;
+
+    float32 impulseMagnitude = numerator / (linearDenomPart);// + rotationalDenomPart);
+    first->velocity = first->velocity + (collisionNormal * (impulseMagnitude / first->mass));
+	second->velocity = second->velocity - (collisionNormal * (impulseMagnitude / second->mass));
+    first->rotationalVelocity = first->rotationalVelocity + firstPerp.dot(collisionNormal * impulseMagnitude) / first->momentOfInertia;
+    second->rotationalVelocity = second->rotationalVelocity - secondPerp.dot(collisionNormal * impulseMagnitude) / second->momentOfInertia;
+}
 
 void update(float32 deltaTimeSeconds, void* userData) {
 	// Update
+	Rectangle rectCopy = rectangle;
 	rectangle.update(deltaTimeSeconds);
+
+	for (int32 segmentIndex = 0; segmentIndex < 4; segmentIndex++) {
+		IntersectionResult ir = getIntersection(&rectangle, &segmentList[segmentIndex]);
+		if (!ir.intersect) {
+		    continue;
+		}
+
+		// Handle collison here
+		IntersectionResult irCopy = ir;
+		float32 copyDt = deltaTimeSeconds;
+		
+		do {
+			ir = irCopy;
+			rectangle = rectCopy;
+			copyDt = copyDt /= 2.f;
+
+			rectangle.update(copyDt);
+			irCopy = getIntersection(&rectangle, &segmentList[segmentIndex]);
+
+			if (copyDt <= 0.f) {
+				printf("Error: Should not be happening.\n");
+				break;
+			}
+
+		} while (irCopy.intersect);
+
+		printf("Found intersection at timestamp: %f\n", copyDt);
+
+		resolveCollision(&rectangle.body, &segmentList[segmentIndex].body, &ir);
+		float32 frameTimeRemaining = deltaTimeSeconds - copyDt;
+		
+		update(frameTimeRemaining, userData);
+		return;
+    }
 	
 	// Renderer
 	renderer.render();