From a264c3af3eadd5683cb2bd3c29cec774e44df7f3 Mon Sep 17 00:00:00 2001 From: James Lambert Date: Tue, 31 May 2022 18:35:57 -0600 Subject: [PATCH] Store information needed to generate contacts --- src/physics/collision_object.c | 48 ++++++++------------------------ src/physics/collision_object.h | 22 +++------------ src/physics/collision_scene.c | 18 ++++++++---- src/physics/epa.c | 50 ++++++++++++++++++++-------------- src/physics/epa.h | 2 +- src/physics/gjk.c | 50 ++++++++++++++++++++++------------ src/physics/gjk.h | 8 ++++-- 7 files changed, 96 insertions(+), 102 deletions(-) diff --git a/src/physics/collision_object.c b/src/physics/collision_object.c index 98594aa..76b7793 100644 --- a/src/physics/collision_object.c +++ b/src/physics/collision_object.c @@ -68,48 +68,22 @@ void collisionObjectUpdateBB(struct CollisionObject* object) { } } -void minkowsiSumAgainstQuadInit(struct MinkowsiSumAgainstQuad* sumData, struct CollisionObject* object, struct CollisionQuad* quad) { - sumData->collisionObject = object; - sumData->quad= quad; -} +void minkowsiSumAgainstQuad(void* data, struct Vector3* direction, struct Vector3* output) { + struct CollisionQuad* quad = (struct CollisionQuad*)data; + *output = quad->corner; -void minkowsiSumAgainstQuadSum(void* data, struct Vector3* direction, struct Vector3* output) { - struct MinkowsiSumAgainstQuad* collisionData = (struct MinkowsiSumAgainstQuad*)data; - - collisionData->collisionObject->collider->callbacks->minkowsiSum(collisionData->collisionObject->collider->data, &collisionData->collisionObject->body->rotationBasis, direction, output); - vector3Add(output, &collisionData->collisionObject->body->transform.position, output); - - struct Vector3 quadSum = collisionData->quad->corner; - - if (vector3Dot(&collisionData->quad->edgeA, direction) < 0.0f) { - vector3AddScaled(&quadSum, &collisionData->quad->edgeA, collisionData->quad->edgeALength, &quadSum); + if (vector3Dot(&quad->edgeA, direction) > 0.0f) { + vector3AddScaled(output, &quad->edgeA, quad->edgeALength, output); } - if (vector3Dot(&collisionData->quad->edgeB, direction) < 0.0f) { - vector3AddScaled(&quadSum, &collisionData->quad->edgeB, collisionData->quad->edgeBLength, &quadSum); + if (vector3Dot(&quad->edgeB, direction) > 0.0f) { + vector3AddScaled(output, &quad->edgeB, quad->edgeBLength, output); } - - vector3Sub(output, &quadSum, output); -} - -void minkowsiSumAgainstObjectsInit(struct MinkowsiSumAgainstObjects* sumData, struct CollisionObject* a, struct CollisionObject* b) { - sumData->collisionObjectA = a; - sumData->collisionObjectB = b; } // data should be of type struct MinkowsiSumAgainstObjects -void minkowsiSumAgainstObjects(void* data, struct Vector3* direction, struct Vector3* output) { - struct MinkowsiSumAgainstObjects* collisionData = (struct MinkowsiSumAgainstObjects*)data; - - collisionData->collisionObjectA->collider->callbacks->minkowsiSum(collisionData->collisionObjectA->collider->data, &collisionData->collisionObjectA->body->rotationBasis, direction, output); - vector3Add(output, &collisionData->collisionObjectA->body->transform.position, output); - - struct Vector3 bResult; - struct Vector3 oppositeDir; - vector3Negate(direction, &oppositeDir); - - collisionData->collisionObjectB->collider->callbacks->minkowsiSum(collisionData->collisionObjectB->collider->data, &collisionData->collisionObjectB->body->rotationBasis, &oppositeDir, output); - vector3Add(output, &collisionData->collisionObjectB->body->transform.position, &bResult); - - vector3Sub(output, &bResult, output); +void minkowsiSumAgainstObject(void* data, struct Vector3* direction, struct Vector3* output) { + struct CollisionObject* object = (struct CollisionObject*)data; + object->collider->callbacks->minkowsiSum(object->collider->data, &object->body->rotationBasis, direction, output); + vector3Add(output, &object->body->transform.position, output); } diff --git a/src/physics/collision_object.h b/src/physics/collision_object.h index f577c62..1ea00e1 100644 --- a/src/physics/collision_object.h +++ b/src/physics/collision_object.h @@ -18,25 +18,11 @@ void collisionObjectCollideWithQuad(struct CollisionObject* object, struct Colli void collisionObjectUpdateBB(struct CollisionObject* object); -struct MinkowsiSumAgainstQuad { - struct CollisionObject* collisionObject; - struct CollisionQuad* quad; -}; +// data should be of type struct CollisionQuad +void minkowsiSumAgainstQuad(void* data, struct Vector3* direction, struct Vector3* output); -void minkowsiSumAgainstQuadInit(struct MinkowsiSumAgainstQuad* sumData, struct CollisionObject* object, struct CollisionQuad* quad); - -// data should be of type struct MinkowsiSumAgainstQuad -void minkowsiSumAgainstQuadSum(void* data, struct Vector3* direction, struct Vector3* output); - -struct MinkowsiSumAgainstObjects { - struct CollisionObject* collisionObjectA; - struct CollisionObject* collisionObjectB; -}; - -void minkowsiSumAgainstObjectsInit(struct MinkowsiSumAgainstObjects* sumData, struct CollisionObject* a, struct CollisionObject* b); - -// data should be of type struct MinkowsiSumAgainstObjects -void minkowsiSumAgainstObjects(void* data, struct Vector3* direction, struct Vector3* output); +// data should be of type struct CollisionObject +void minkowsiSumAgainstObject(void* data, struct Vector3* direction, struct Vector3* output); #endif \ No newline at end of file diff --git a/src/physics/collision_scene.c b/src/physics/collision_scene.c index b6e0dd6..c1d7773 100644 --- a/src/physics/collision_scene.c +++ b/src/physics/collision_scene.c @@ -392,19 +392,27 @@ int collisionSceneTestMinkowsiSum(struct CollisionObject* object) { return 0; } - struct MinkowsiSumAgainstQuad quadSum; - quadSum.collisionObject = object; struct Simplex simplex; short colliderIndices[MAX_COLLIDERS]; int quadCount = collisionObjectRoomColliders(&gCollisionScene.world->rooms[object->body->currentRoom], &object->boundingBox, colliderIndices); for (int i = 0; i < quadCount; ++i) { - quadSum.quad = gCollisionScene.quads[colliderIndices[i]].collider->data; + struct CollisionQuad* quad = gCollisionScene.quads[colliderIndices[i]].collider->data; - if (box3DHasOverlap(&object->boundingBox, &gCollisionScene.quads[colliderIndices[i]].boundingBox) && gjkCheckForOverlap(&simplex, &quadSum, minkowsiSumAgainstQuadSum, &quadSum.quad->plane.normal)) { + if (box3DHasOverlap(&object->boundingBox, &gCollisionScene.quads[colliderIndices[i]].boundingBox) && + gjkCheckForOverlap( + &simplex, + object, minkowsiSumAgainstObject, + quad, minkowsiSumAgainstQuad, + &quad->plane.normal)) { struct EpaResult result; - epaSolve(&simplex, &quadSum, minkowsiSumAgainstQuadSum, &result); + epaSolve( + &simplex, + object, minkowsiSumAgainstObject, + quad, minkowsiSumAgainstQuad, + &result + ); return 1; } } diff --git a/src/physics/epa.c b/src/physics/epa.c index 57bfc92..8e2e895 100644 --- a/src/physics/epa.c +++ b/src/physics/epa.c @@ -16,22 +16,20 @@ struct SimplexTriangle { struct ExpandingSimplex { struct Vector3 points[MAX_SIMPLEX_POINTS]; + struct Vector3 aPoints[MAX_SIMPLEX_POINTS]; + struct Vector3 bPoints[MAX_SIMPLEX_POINTS]; unsigned pointCount; struct SimplexTriangle triangles[MAX_SIMPLEX_TRIANGLES]; unsigned triangleCount; unsigned char triangleHeap[MAX_SIMPLEX_TRIANGLES]; }; -int expandingSimplexAddPoint(struct ExpandingSimplex* simplex, struct Vector3* point) { - if (simplex->pointCount == MAX_SIMPLEX_POINTS) { - return -1; - } - +void expandingSimplexAddPoint(struct ExpandingSimplex* simplex, struct Vector3* aPoint, struct Vector3* bPoint, struct Vector3* pointDiff) { int result = simplex->pointCount; - simplex->points[result] = *point; + simplex->aPoints[result] = *aPoint; + simplex->bPoints[result] = *bPoint; + simplex->points[result] = *pointDiff; ++simplex->pointCount; - - return result; } void expandingSimplexTriangleInit(struct ExpandingSimplex* simplex, int a, int b, int c, struct SimplexTriangle* triangle) { @@ -143,10 +141,9 @@ void expandingSimplexInit(struct ExpandingSimplex* expandingSimplex, struct Simp expandingSimplex->triangleCount = 0; expandingSimplex->pointCount = 0; - expandingSimplexAddPoint(expandingSimplex, &simplex->points[0]); - expandingSimplexAddPoint(expandingSimplex, &simplex->points[1]); - expandingSimplexAddPoint(expandingSimplex, &simplex->points[2]); - expandingSimplexAddPoint(expandingSimplex, &simplex->points[3]); + for (int i = 0; i < 4; ++i) { + expandingSimplexAddPoint(expandingSimplex, &simplex->objectAPoint[i], &simplex->objectBPoint[i], &simplex->points[i]); + } expandingSimplexAddTriangle(expandingSimplex, 0, 1, 2); expandingSimplexAddTriangle(expandingSimplex, 2, 1, 3); @@ -154,9 +151,7 @@ void expandingSimplexInit(struct ExpandingSimplex* expandingSimplex, struct Simp expandingSimplexAddTriangle(expandingSimplex, 1, 0, 3); } -int expandingSimplexExpand(struct ExpandingSimplex* expandingSimplex, struct Vector3* newPoint) { - int newPointIndex = expandingSimplexAddPoint(expandingSimplex, newPoint); - +int expandingSimplexExpand(struct ExpandingSimplex* expandingSimplex, int newPointIndex) { if (newPointIndex == -1) { return 0; } @@ -177,26 +172,39 @@ int expandingSimplexExpand(struct ExpandingSimplex* expandingSimplex, struct Vec return 1; } -void epaSolve(struct Simplex* startingSimplex, void* data, MinkowsiSum sum, struct EpaResult* result) { +void epaSolve(struct Simplex* startingSimplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct EpaResult* result) { struct ExpandingSimplex* simplex = stackMalloc(sizeof(struct ExpandingSimplex)); expandingSimplexInit(simplex, startingSimplex); - struct Vector3 nextPoint; struct SimplexTriangle* closestFace = NULL; float projection = 0.0f; int i; for (i = 0; i < MAX_ITERATIONS; ++i) { - closestFace = expandingSimplexClosestFace(simplex); - sum(data, &closestFace->normal, &nextPoint); + struct Vector3 reverseNormal; - projection = vector3Dot(&nextPoint, &closestFace->normal); + closestFace = expandingSimplexClosestFace(simplex); + + int nextIndex = simplex->pointCount; + + struct Vector3* aPoint = &simplex->aPoints[nextIndex]; + struct Vector3* bPoint = &simplex->bPoints[nextIndex]; + + objectASum(objectA, &closestFace->normal, aPoint); + vector3Negate(&closestFace->normal, &reverseNormal); + objectBSum(objectB, &reverseNormal, bPoint); + + vector3Sub(aPoint, bPoint, &simplex->points[nextIndex]); + + projection = vector3Dot(&simplex->points[nextIndex], &closestFace->normal); if ((projection - closestFace->distanceToOrigin) < 0.00000001f) { break; } - if (!expandingSimplexExpand(simplex, &nextPoint)) { + ++simplex->pointCount; + + if (!expandingSimplexExpand(simplex, nextIndex)) { break; } } diff --git a/src/physics/epa.h b/src/physics/epa.h index 1d5553d..048a25b 100644 --- a/src/physics/epa.h +++ b/src/physics/epa.h @@ -8,6 +8,6 @@ struct EpaResult { float penetration; }; -void epaSolve(struct Simplex* startingSimplex, void* data, MinkowsiSum sum, struct EpaResult* result); +void epaSolve(struct Simplex* startingSimplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct EpaResult* result); #endif \ No newline at end of file diff --git a/src/physics/gjk.c b/src/physics/gjk.c index 602c5f1..219d1e1 100644 --- a/src/physics/gjk.c +++ b/src/physics/gjk.c @@ -1,20 +1,23 @@ #include "gjk.h" -void simplexInit(struct Simplex* simplex, struct Vector3* firstPoint) { - simplex->nPoints = 1; - simplex->points[0] = *firstPoint; +void simplexInit(struct Simplex* simplex) { + simplex->nPoints = 0; } -int simplexAddPoint(struct Simplex* simplex, struct Vector3* point) { +struct Vector3* simplexAddPoint(struct Simplex* simplex, struct Vector3* aPoint, struct Vector3* bPoint) { if (simplex->nPoints == MAX_SIMPLEX_SIZE) { // SHOULD never happen, but just in case return 0; } - simplex->points[simplex->nPoints] = *point; + int index = simplex->nPoints; + + simplex->objectAPoint[index] = *aPoint; + simplex->objectBPoint[index] = *bPoint; + vector3Sub(&simplex->objectAPoint[index], &simplex->objectBPoint[index], &simplex->points[index]); ++simplex->nPoints; - return 1; + return &simplex->points[index]; } int simplexCheck(struct Simplex* simplex, struct Vector3* nextDirection) { @@ -155,30 +158,43 @@ int simplexCheck(struct Simplex* simplex, struct Vector3* nextDirection) { #define MAX_GJK_ITERATIONS 10 -int gjkCheckForOverlap(struct Simplex* simplex, void* data, MinkowsiSum sum, struct Vector3* firstDirection) { - struct Vector3 nextPoint; +int gjkCheckForOverlap(struct Simplex* simplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct Vector3* firstDirection) { + struct Vector3 aPoint; + struct Vector3 bPoint; struct Vector3 nextDirection; + simplexInit(simplex); + if (vector3IsZero(firstDirection)) { - sum(data, &gRight, &nextPoint); + objectASum(objectA, &gRight, &aPoint); vector3Negate(&gRight, &nextDirection); + + objectBSum(objectB, &nextDirection, &bPoint); + simplexAddPoint(simplex, &aPoint, &bPoint); } else { - sum(data, firstDirection, &nextPoint); + objectASum(objectA, firstDirection, &aPoint); vector3Negate(firstDirection, &nextDirection); + + objectBSum(objectB, &nextDirection, &bPoint); + simplexAddPoint(simplex, &aPoint, &bPoint); } - simplexInit(simplex, &nextPoint); - for (int iteration = 0; iteration < MAX_GJK_ITERATIONS; ++iteration) { - sum(data, &nextDirection, &nextPoint); + struct Vector3 reverseDirection; + vector3Negate(&nextDirection, &reverseDirection); + objectASum(objectA, &nextDirection, &aPoint); + objectBSum(objectB, &reverseDirection, &bPoint); + + struct Vector3* addedPoint = simplexAddPoint(simplex, &aPoint, &bPoint); + + if (!addedPoint) { + return 0; + } - if (vector3Dot(&nextPoint, &nextDirection) <= 0.0f) { + if (vector3Dot(addedPoint, &nextDirection) <= 0.0f) { return 0; } - if (!simplexAddPoint(simplex, &nextPoint)) { - return 0; - } if (simplexCheck(simplex, &nextDirection)) { return 1; diff --git a/src/physics/gjk.h b/src/physics/gjk.h index d006dc4..8084b41 100644 --- a/src/physics/gjk.h +++ b/src/physics/gjk.h @@ -9,13 +9,15 @@ typedef void (*MinkowsiSum)(void* data, struct Vector3* direction, struct Vector struct Simplex { struct Vector3 points[MAX_SIMPLEX_SIZE]; + struct Vector3 objectAPoint[MAX_SIMPLEX_SIZE]; + struct Vector3 objectBPoint[MAX_SIMPLEX_SIZE]; short nPoints; }; -void simplexInit(struct Simplex* simplex, struct Vector3* firstPoint); -int simplexAddPoint(struct Simplex* simplex, struct Vector3* point); +void simplexInit(struct Simplex* simplex); +struct Vector3* simplexAddPoint(struct Simplex* simplex, struct Vector3* aPoint, struct Vector3* bPoint); int simplexCheck(struct Simplex* simplex, struct Vector3* nextDirection); -int gjkCheckForOverlap(struct Simplex* simplex, void* data, MinkowsiSum sum, struct Vector3* firstDirection); +int gjkCheckForOverlap(struct Simplex* simplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct Vector3* firstDirection); #endif \ No newline at end of file