From a003378e602107d421b819ac629493b5c6444762 Mon Sep 17 00:00:00 2001 From: Matthew Kosarek Date: Tue, 19 Oct 2021 07:10:49 -0400 Subject: Working sphere sphere intersection --- 3d/rigidbody/dist/output.wasm | Bin 56137 -> 62008 bytes 3d/rigidbody/main.cpp | 117 ++++++++++++++++++++++++++++++++++++++---- 2 files changed, 108 insertions(+), 9 deletions(-) diff --git a/3d/rigidbody/dist/output.wasm b/3d/rigidbody/dist/output.wasm index 3702497..1bc13cc 100755 Binary files a/3d/rigidbody/dist/output.wasm and b/3d/rigidbody/dist/output.wasm differ diff --git a/3d/rigidbody/main.cpp b/3d/rigidbody/main.cpp index bf6af58..f95e634 100644 --- a/3d/rigidbody/main.cpp +++ b/3d/rigidbody/main.cpp @@ -13,6 +13,7 @@ #include #include +// -- Rigidbody updates struct Impulse { Vector3 force = { 0, 0, 0 }; Vector3 pointOfApplication = { 0, 0, 0 }; @@ -33,6 +34,7 @@ struct Rigidbody3d { Vector3 rotationalVelocity; Quaternion rotation; float32 momentOfInertia = 1.f; + float32 cofOfRestitution = 1.f; void reset() { numImpulses = 0; @@ -100,14 +102,16 @@ struct Rigidbody3d { } }; +// -- Sphere definition float32 bounds = 20.f; struct Sphere { Mesh3d mesh; + Rigidbody3d previousBody; Rigidbody3d body; + float32 radius = 5.f; void load(Renderer3d* renderer) { - const float32 scale = 3.f; const float32 angleIncrements = 2.f; const int32 numFaces = static_cast((180.f / angleIncrements + 1) * (360.f / angleIncrements + 1)); const int32 numVertices = 4.f * numFaces; @@ -135,22 +139,22 @@ struct Sphere { const auto nextCosTheta = cos(DEG_TO_RAD(theta + angleIncrements)); // Top Left Point - auto topLeftPoint = Vector3(scale * sinPhi * cosTheta, scale * sinPhi * sinTheta, scale * cosPhi); + auto topLeftPoint = Vector3(radius * sinPhi * cosTheta, radius * sinPhi * sinTheta, radius * cosPhi); auto topLeftIdx = index++; vertices[vidx++] = { topLeftPoint, topLeftPoint.normalize(), color }; // Bottom Left Point - auto bottomLeftPoint = Vector3(scale * nextSinPhi * cosTheta, scale * nextSinPhi * sinTheta, scale * nextCosPhi); + auto bottomLeftPoint = Vector3(radius * nextSinPhi * cosTheta, radius * nextSinPhi * sinTheta, radius * nextCosPhi); auto bottomLeftIdx = index++; vertices[vidx++] = { bottomLeftPoint, bottomLeftPoint.normalize(), color }; // Bottom Right Point - auto bottomRightPoint = Vector3(scale * nextSinPhi * nextCosTheta, scale * nextSinPhi * nextSinTheta, scale * nextCosPhi); + auto bottomRightPoint = Vector3(radius * nextSinPhi * nextCosTheta, radius * nextSinPhi * nextSinTheta, radius * nextCosPhi); auto bottomRightIdx = index++; vertices[vidx++] = { bottomRightPoint, bottomRightPoint.normalize(), color }; // Top Right Point - auto topRightPoint = Vector3(scale * sinPhi * nextCosTheta, scale * sinPhi * nextSinTheta, scale * cosPhi); + auto topRightPoint = Vector3(radius * sinPhi * nextCosTheta, radius * sinPhi * nextSinTheta, radius * cosPhi); auto topRightIdx = index++; vertices[vidx++] = { topRightPoint, topRightPoint.normalize(), color }; @@ -168,7 +172,7 @@ struct Sphere { body.position = Vector3 { 0.f, 0.f, 0.f }; body.velocity = Vector3 { 0.f, 0.f, 0.f }; - float32 singleFaceArea = scale; + float32 singleFaceArea = radius; body.momentOfInertia = (body.mass * singleFaceArea) / 6.f; delete [] vertices; @@ -176,8 +180,10 @@ struct Sphere { } void update(float32 dtSeconds) { + previousBody = body; body.update(dtSeconds); + // -- Constrain inside of the box if (body.position.x > bounds) { body.position.x = bounds; body.velocity.x = -body.velocity.x; @@ -218,11 +224,23 @@ struct Sphere { } }; +// -- Intersection info +struct IntersectionResult { + bool intersect = false; + Vector3 collisionNormal; + Vector3 relativeVelocity; + Vector3 firstPointOfApplication; + Vector3 secondPointOfApplication; +}; + + EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData); EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData); EM_BOOL onForceApplicationRequested(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData); void load(); +IntersectionResult getIntersection(Sphere* first, Sphere* second); +void resolveIntersection(Rigidbody3d* first, Rigidbody3d* second, IntersectionResult* ir); void update(float32 time, void* userData); void unload(); @@ -255,20 +273,101 @@ void load() { sphereList[1].load(&renderer); sphereList[1].body.mass = 6.f; sphereList[1].body.position = Vector3(-10.f, 0, 5); - sphereList[1].body.velocity = Vector3(10.f, 10.f, -10.f); + sphereList[1].body.velocity = Vector3(10.f, -10.f, -10.f); sphereList[1].body.rotationalVelocity = Vector3(1.f, 1.f, 1.f); camera.projection = Mat4x4().getPerspectiveProjection(0.1f, 10000.f, DEG_TO_RAD(45.f), 800.f / 600.f); - camera.view = Mat4x4().translate({ 0, 0, -40.f }); + camera.view = Mat4x4().translate({ 0, 0, -60.f }); mainLoop.run(update); } +IntersectionResult getIntersection(Sphere* first, Sphere* second) { + IntersectionResult ir; + auto distance = (first->body.position - second->body.position).length(); + if (distance <= first->radius + second->radius) { + ir.intersect = true; + } + else { + ir.intersect = false; + return ir; + } + + auto positionDiff = first->body.position - second->body.position; + auto positionDirection = positionDiff.normalize(); + ir.relativeVelocity = first->body.velocity - second->body.velocity; + ir.firstPointOfApplication = positionDirection * first->radius; + ir.secondPointOfApplication = positionDirection * second->radius; + ir.collisionNormal = (positionDirection + ir.relativeVelocity.negate().normalize()).normalize(); + + return ir; +} + +void resolveIntersection(Rigidbody3d* first, Rigidbody3d* second, IntersectionResult* ir) { + Vector3 relativeVelocity = ir->relativeVelocity; + Vector3 collisionNormal = ir->collisionNormal; + Vector3 firstPerp = ir->firstPointOfApplication.cross(relativeVelocity); + Vector3 secondPerp = ir->secondPointOfApplication.cross(relativeVelocity); + 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) { + // -- Update for (int32 idx = 0; idx < numSpheres; idx++) { sphereList[idx].update(deltaTimeSeconds); } - // Renderer + // -- Check collisions + for (int32 idx = 0; idx < numSpheres; idx++) { + auto first = &sphereList[idx]; + for (int32 otherIdx = idx + 1; otherIdx < numSpheres; otherIdx++) { + auto second = &sphereList[otherIdx]; + + auto ir = getIntersection(first, second); + if (!ir.intersect) { continue; } + + // Find out where the intersection took place. We will rewind the simulation + // until the spheres are no longer intersecting. + const float32 numDtSubdivision = 8.f; + const float32 subdivisionSeconds = deltaTimeSeconds / numDtSubdivision; + int32 currentDtSubdivision = 1.f; + IntersectionResult tempIr = ir; + do { + ir = tempIr; + + // Restore the bodies to the point before the collision happened + first->body = first->previousBody; + second->body = second->previousBody; + + // Subdivide the timestep to get a more granular intersection result + float32 currentUpdateStep = deltaTimeSeconds - (currentDtSubdivision * subdivisionSeconds); + + first->update(currentUpdateStep); + second->update(currentUpdateStep); + + tempIr = getIntersection(first, second); + currentDtSubdivision++; + } while (tempIr.intersect); + + // Exact intersection point is found. Now we can solve it + resolveIntersection(&first->body, &second->body, &ir); + } + } + + // -- Render renderer.render(&camera); for (int32 idx = 0; idx < numSpheres; idx++) { sphereList[idx].render(&renderer); -- cgit v1.2.1