From a0419a14c0d34976587e830fd780e50bac66eb6a Mon Sep 17 00:00:00 2001 From: Matthew Kosarek Date: Sat, 29 May 2021 11:31:09 -0400 Subject: Beginning of rigidbody tutorial #3 --- 2d/rigidbody/rigidbody_3/main.cpp | 208 +++++++++++++++++++++++++++++++++++++- 1 file changed, 205 insertions(+), 3 deletions(-) (limited to '2d/rigidbody/rigidbody_3/main.cpp') diff --git a/2d/rigidbody/rigidbody_3/main.cpp b/2d/rigidbody/rigidbody_3/main.cpp index 06f77dc..2063718 100644 --- a/2d/rigidbody/rigidbody_3/main.cpp +++ b/2d/rigidbody/rigidbody_3/main.cpp @@ -21,6 +21,7 @@ struct Rigidbody { float32 rotationalVelocity = 0.f; float32 rotation = 0.f; float32 momentOfInertia = 1.f; + float32 cofOfRestitution = 1.f; void reset() { linearForce = { 0, 0 }; @@ -54,11 +55,19 @@ struct Rigidbody { } }; +struct Edge { + Vector2 normal; + Vector2 start; + Vector2 end; +}; + struct Rectangle { OrthographicShape shape; Rigidbody body; + Rigidbody previousBody; Vector2 originalPoints[4]; Vector2 transformedPoints[4]; + Edge edges[4]; void load(OrthographicRenderer* renderer, Vector4 color, float32 width, float32 height) { color = color.toNormalizedColor(); @@ -90,14 +99,26 @@ struct Rectangle { } void update(float32 dtSeconds) { + previousBody = body; body.update(dtSeconds); shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation); - // Note: This helps us check if the cursor is within the bounds of the - // rectangle later on. + // Note: This helps us check rectangle collisions using SAT later on. + // This is probably a slightly slow way of doing this, but we will ignore + // that for now. for (int idx = 0; idx < 4; idx++) { transformedPoints[idx] = shape.model * originalPoints[idx]; } + + for (int eidx = 0; eidx < 4; eidx++) { + edges[eidx].start = transformedPoints[eidx]; + edges[eidx].end = transformedPoints[eidx == 3 ? 0 : eidx + 1]; + edges[eidx].normal = (edges[eidx].end - edges[eidx].start).getPerp().normalize(); + } + } + + void restorePreviousBody() { + body = previousBody; } void render(OrthographicRenderer* renderer) { @@ -109,6 +130,13 @@ struct Rectangle { } }; +struct IntersectionResult { + bool intersect = false; + Vector2 collisionNormal; + Vector2 relativeVelocity; + Vector2 firstPointOfApplication; + Vector2 secondPointOfApplication; +}; EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData); EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData); @@ -165,13 +193,187 @@ void handleCollisionWithWall(Rectangle* r) { } } +Vector2 getProjection(Vector2* vertices, Vector2 axis) { + float32 min = axis.dot(vertices[0]); + float32 max = min; + + for (int v = 1; v < 4; v++) { + float32 d = axis.dot(vertices[v]); + + if (d < min) { + min = d; + } else if (d > max) { + max = d; + } + } + + return Vector2 { min, max }; +} + +bool projectionsOverlap(Vector2 first, Vector2 second) { + return first.x <= second.y && second.x <= first.y; +} + +float32 getProjectionOverlap(Vector2 first, Vector2 second) { + float32 firstOverlap = fabs(first.x - second.y); + float32 secondOverlap = fabs(second.x - first.y); + return firstOverlap > secondOverlap ? secondOverlap : firstOverlap; +} + +const float32 EPSILON = 1.f; +IntersectionResult getIntersection(Rectangle* first, Rectangle* second) { + IntersectionResult ir; + + // For two rectangles to overlap, it means that at least one of the corners of one is inside of the other + Edge* firstEdges = first->edges; + Vector2* firstPoints = first->transformedPoints; + + Edge* secondEdges = second->edges; + Vector2* secondPoints = second->transformedPoints; + + float32 minOverlap = FLT_MAX; + Vector2 minOverlapAxis; + Edge* minOverlapEdge = NULL; + bool minOverlapWasFirstRect = false; + + for (int i = 0; i < 4; i++) { + Vector2 normal = firstEdges[i].normal; + + Vector2 firstProj = getProjection(firstPoints, normal); + Vector2 secondProj = getProjection(secondPoints, normal); + + if (!projectionsOverlap(firstProj, secondProj)) { + return ir; + } + + float32 overlap = getProjectionOverlap(firstProj, secondProj); + if (overlap < minOverlap) { + minOverlap = overlap; + minOverlapAxis = normal; + minOverlapEdge = &firstEdges[i]; + minOverlapWasFirstRect = true; + } + } + + for (int i = 0; i < 4; i++) { + Vector2 normal = secondEdges[i].normal; + + Vector2 firstProj = getProjection(firstPoints, normal); + Vector2 secondProj = getProjection(secondPoints, normal); + + if (!projectionsOverlap(firstProj, secondProj)) { + return ir; + } + + float32 overlap = getProjectionOverlap(firstProj, secondProj); + if (overlap < minOverlap) { + minOverlap = overlap; + minOverlapAxis = normal; + minOverlapEdge = &secondEdges[i]; + } + } + + ir.intersect = true; + ir.relativeVelocity = first->body.velocity - second->body.velocity; + ir.collisionNormal = minOverlapAxis; + + // Find the point of collision, this is kind of tricky, and it is just an approximation for now. + // At this point, we know that we intersected along the minOverlapAxis, but we do not know where + // that exactly happened. To remedy this will, we create two parallel lines: one at the top of the + // normal area, and one at the bottom. For point on both of the Rectangles, we will check: + // (1) if it is between these two planes + // (2) if, for that rectangle, it is the closest point to the original normal vector + // (3) or if it is equally distant from normal vector as another point (then this is a "flat" collision) + // + // The collision point MUST be between these two planes. We can then say the corner/face of the non-monoverlapAxis + // Rectangle is the collision point. This enables us to then solve for their respective points of application fairly + // easily. If the collision "point" is an entire face, we make the collision point be the center point. + // + + Vector2 closestPoint; + float32 minDistance = FLT_MAX; + + for (int p = 0; p < 4; p++) { + Vector2 point = minOverlapWasFirstRect ? secondPoints[p] : firstPoints[p]; + + float32 distFromPointToStart = (minOverlapEdge->start - point).length(); + float32 distFromPointToEnd = (minOverlapEdge->end - point).length(); + float32 potentialMin = MIN(distFromPointToStart, distFromPointToEnd); + + if (potentialMin < minDistance) { + closestPoint = point; + minDistance = potentialMin; + } + } + + ir.firstPointOfApplication = closestPoint - first->body.position; + ir.secondPointOfApplication = closestPoint - second->body.position;; + + 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 cofOfRestitution = (first->cofOfRestitution + second->cofOfRestitution) / 2.f; + float32 numerator = (relativeVelocity * (-1 * (1.f + cofOfRestitution))).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) { r1.update(deltaTimeSeconds); r2.update(deltaTimeSeconds); + // Handle intersection between the two rectangles here + IntersectionResult ir = getIntersection(&r1, &r2); + if (ir.intersect) { + IntersectionResult irCopy = ir; + float32 copyDt = deltaTimeSeconds; + + do { + r1.restorePreviousBody(); + r2.restorePreviousBody(); + + ir = irCopy; + copyDt = copyDt /= 2.f; + + r1.update(copyDt); + r2.update(copyDt); + + irCopy = getIntersection(&r1, &r2); + + if (copyDt <= 0.f) { + printf("Error: Should not be happening.\n"); + break; + } + + } while (irCopy.intersect); + + printf("Found intersection at timestamp: %f\n", copyDt); + + resolveCollision(&r1.body, &r2.body, &ir); + float32 frameTimeRemaining = deltaTimeSeconds - copyDt; + + r1.update(frameTimeRemaining); + r2.update(frameTimeRemaining); + } + + // Keep within the bounds handleCollisionWithWall(&r1); handleCollisionWithWall(&r2); - // Renderer renderer.render(); -- cgit v1.2.1