From 7ca5660e118db39360802c231c6489ac967dd4b8 Mon Sep 17 00:00:00 2001 From: James Lambert Date: Mon, 18 Apr 2022 21:59:37 -0600 Subject: [PATCH] Work on sphere collision --- src/levels/levels.c | 8 +-- src/levels/levels.h | 3 +- src/main.c | 3 + src/physics/collision.h | 1 + src/physics/collision_quad.c | 34 +++++++---- src/physics/collision_quad.h | 1 + src/physics/collision_scene.c | 14 +++++ src/physics/collision_scene.h | 16 +++++ src/physics/collision_sphere.c | 105 +++++++++++++++++++++++++++++++++ src/physics/collision_sphere.h | 17 ++++++ src/scene/cube.c | 8 +-- 11 files changed, 184 insertions(+), 26 deletions(-) create mode 100644 src/physics/collision_scene.c create mode 100644 src/physics/collision_scene.h create mode 100644 src/physics/collision_sphere.c create mode 100644 src/physics/collision_sphere.h diff --git a/src/levels/levels.c b/src/levels/levels.c index 0f0dfd1..4aa384e 100644 --- a/src/levels/levels.c +++ b/src/levels/levels.c @@ -2,10 +2,8 @@ #include "../build/assets/test_chambers/level_list.h" -struct CollisionObject* levelsGetCollisionObjects() { - return test_chamber_00_test_chamber_00_0_collision_objects; -} +#include "physics/collision_scene.h" -int levelsGetCollisionObjectCount() { - return TEST_CHAMBER_00_TEST_CHAMBER_00_0_QUAD_COLLIDERS_COUNT; +void levelLoadCollisionScene() { + collisionSceneInit(&gCollisionScene, test_chamber_00_test_chamber_00_0_collision_objects, TEST_CHAMBER_00_TEST_CHAMBER_00_0_QUAD_COLLIDERS_COUNT); } \ No newline at end of file diff --git a/src/levels/levels.h b/src/levels/levels.h index bb4b28e..9b095ad 100644 --- a/src/levels/levels.h +++ b/src/levels/levels.h @@ -3,7 +3,6 @@ #include "physics/collision_object.h" -struct CollisionObject* levelsGetCollisionObjects(); -int levelsGetCollisionObjectCount(); +void levelLoadCollisionScene(); #endif \ No newline at end of file diff --git a/src/main.c b/src/main.c index 4398a7b..f2de973 100644 --- a/src/main.c +++ b/src/main.c @@ -11,6 +11,8 @@ #include "string.h" #include "controls/controller.h" +#include "levels/levels.h" + #ifdef WITH_DEBUGGER #include "../debugger/debugger.h" #endif @@ -130,6 +132,7 @@ static void gameProc(void* arg) { sceneInit(&gScene); romInit(); controllersInit(); + levelLoadCollisionScene(); #ifdef WITH_DEBUGGER OSThread* debugThreads[2]; debugThreads[0] = &gameThread; diff --git a/src/physics/collision.h b/src/physics/collision.h index 9ee18e2..0cb31cd 100644 --- a/src/physics/collision.h +++ b/src/physics/collision.h @@ -10,6 +10,7 @@ enum CollisionShapeType { CollisionShapeTypeBox, CollisionShapeTypeQuad, + CollisionShapeTypeSphere, }; struct ContactPoint { diff --git a/src/physics/collision_quad.c b/src/physics/collision_quad.c index 296a02f..89b59a6 100644 --- a/src/physics/collision_quad.c +++ b/src/physics/collision_quad.c @@ -17,20 +17,9 @@ * edge a --> */ -int _collsionBuildQuadContact(struct Transform* boxTransform, struct CollisionQuad* quad, struct Vector3* point, struct ContactConstraintState* output, int id) { - 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(&quad->plane, &worldPoint); - - if (penetration >= NEGATIVE_PENETRATION_BIAS) { - return POINT_NO_OVERLAP; - } - +int collisionQuadDetermineEdges(struct Vector3* worldPoint, struct CollisionQuad* quad) { struct Vector3 relative; - vector3Sub(&worldPoint, &quad->corner, &relative); + vector3Sub(worldPoint, &quad->corner, &relative); float edgeDistance = vector3Dot(&relative, &quad->edgeA); int edgeMask = 0; @@ -53,6 +42,23 @@ int _collsionBuildQuadContact(struct Transform* boxTransform, struct CollisionQu edgeMask |= 1 << 3; } + return edgeMask; +} + +int _collsionBuildQuadContact(struct Transform* boxTransform, struct CollisionQuad* quad, struct Vector3* point, struct ContactConstraintState* output, int id) { + 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(&quad->plane, &worldPoint); + + if (penetration >= NEGATIVE_PENETRATION_BIAS) { + return POINT_NO_OVERLAP; + } + + int edgeMask = collisionQuadDetermineEdges(&worldPoint, quad); + if (edgeMask) { return edgeMask; } @@ -389,6 +395,7 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C struct Vector3 nextFurthestPoint = deepestCorner; + // TODO roll up into a loop int nextId = id ^ (1 << minAxis); VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis]; int pointEdges = _collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId); @@ -438,6 +445,7 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C * edge a --> */ + // TODO roll up into a loop if (edges & (1 << 0)) { edge.origin = localOrigin; edge.direction = localEdgeB; diff --git a/src/physics/collision_quad.h b/src/physics/collision_quad.h index ff9c60b..ba17e4f 100644 --- a/src/physics/collision_quad.h +++ b/src/physics/collision_quad.h @@ -19,6 +19,7 @@ struct CollisionQuad { u8 enabledEdges; }; +int collisionQuadDetermineEdges(struct Vector3* worldPoint, struct CollisionQuad* quad); int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactConstraintState* output); #endif \ No newline at end of file diff --git a/src/physics/collision_scene.c b/src/physics/collision_scene.c new file mode 100644 index 0000000..6160abd --- /dev/null +++ b/src/physics/collision_scene.c @@ -0,0 +1,14 @@ +#include "collision_scene.h" + +struct CollisionScene gCollisionScene; + +void collisionSceneInit(struct CollisionScene* scene, struct CollisionObject* quads, int quadCount) { + scene->quads = quads; + scene->quadCount = quadCount; +} + +void collisionObjectCollideWithScene(struct CollisionObject* object, struct CollisionScene* scene, struct ContactSolver* contactSolver) { + for (int i = 0; i < scene->quadCount; ++i) { + collisionObjectCollideWithQuad(object, &scene->quads[i], contactSolver); + } +} \ No newline at end of file diff --git a/src/physics/collision_scene.h b/src/physics/collision_scene.h new file mode 100644 index 0000000..fbafb23 --- /dev/null +++ b/src/physics/collision_scene.h @@ -0,0 +1,16 @@ +#ifndef __COLLISION_SCENE_H__ +#define __COLLISION_SCENE_H__ + +#include "collision_object.h" + +struct CollisionScene { + struct CollisionObject* quads; + int quadCount; +}; + +extern struct CollisionScene gCollisionScene; + +void collisionSceneInit(struct CollisionScene* scene, struct CollisionObject* quads, int quadCount); +void collisionObjectCollideWithScene(struct CollisionObject* object, struct CollisionScene* scene, struct ContactSolver* contactSolver); + +#endif \ No newline at end of file diff --git a/src/physics/collision_sphere.c b/src/physics/collision_sphere.c new file mode 100644 index 0000000..c278c09 --- /dev/null +++ b/src/physics/collision_sphere.c @@ -0,0 +1,105 @@ +#include "collision_sphere.h" + +#include "math/plane.h" +#include "math/mathf.h" +#include "collision_quad.h" + +int collisionSphereCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactConstraintState* output) { + struct CollisionSphere* sphere = (struct CollisionSphere*)data; + + float overlap = planePointDistance(&quad->plane, &boxTransform->position) - sphere->radius; + + if (overlap > NEGATIVE_PENETRATION_BIAS || overlap < -2.0 * sphere->radius) { + return 0; + } + + struct Vector3 relativePos; + vector3Sub(&boxTransform->position, &quad->corner, &relativePos); + + float aLerp = clampf(vector3Dot(&relativePos, &quad->edgeA), 0.0f, quad->edgeALength); + float bLerp = clampf(vector3Dot(&relativePos, &quad->edgeB), 0.0f, quad->edgeBLength); + + struct ContactState* contact = &output->contacts[output->contactCount]; + + vector3AddScaled(&quad->corner, &quad->edgeA, aLerp, &contact->ra); + vector3AddScaled(&contact->ra, &quad->edgeB, bLerp, &contact->ra); + + vector3Sub(&boxTransform->position, &contact->rb, &output->normal); + + float outputLength = vector3MagSqrd(&output->normal); + + float extraRadius = sphere->radius + NEGATIVE_PENETRATION_BIAS; + + if (outputLength * outputLength > extraRadius * extraRadius || vector3Dot(&output->normal, &quad->plane.normal) < 0.0f) { + return 0; + } + + vector3Scale(&output->normal, &output->normal, 1.0f / sqrtf(outputLength)); + + vector3AddScaled(&boxTransform->position, &output->normal, -sphere->radius, &contact->rb); + + output->restitution = 0.1f; + output->friction = 0.5f; + ++output->contactCount; + // TODO + output->tangentVectors[0] = quad->edgeA; + output->tangentVectors[1] = quad->edgeB; + + contact->id = 0; + contact->penetration = overlap; + contact->bias = 0; + contact->normalMass = 0; + contact->tangentMass[0] = 0.0f; + contact->tangentMass[1] = 0.0f; + contact->normalImpulse = 0.0f; + contact->tangentImpulse[0] = 0.0f; + contact->tangentImpulse[1] = 0.0f; + + return 1; +} + +int collisionSphereCollidePlane(void* data, struct Transform* boxTransform, struct Plane* plane, struct ContactConstraintState* output) { + struct CollisionSphere* sphere = (struct CollisionSphere*)data; + + float overlap = planePointDistance(plane, &boxTransform->position) - sphere->radius; + + if (overlap > NEGATIVE_PENETRATION_BIAS || overlap < -2.0 * sphere->radius) { + return 0; + } + + struct ContactState* contact = &output->contacts[output->contactCount]; + + planeProjectPoint(plane, &boxTransform->position, &contact->ra); + output->normal = plane->normal; + vector3AddScaled(&boxTransform->position, &output->normal, -sphere->radius, &contact->rb); + + output->restitution = 0.1f; + output->friction = 0.5f; + ++output->contactCount; + // TODO + output->tangentVectors[0] = gRight; + output->tangentVectors[1] = gForward; + + contact->id = 0; + contact->penetration = overlap; + contact->bias = 0; + contact->normalMass = 0; + contact->tangentMass[0] = 0.0f; + contact->tangentMass[1] = 0.0f; + contact->normalImpulse = 0.0f; + contact->tangentImpulse[0] = 0.0f; + contact->tangentImpulse[1] = 0.0f; + + return 1; +} + +float collisionSphereSolidMofI(struct ColliderTypeData* typeData, float mass) { + struct CollisionSphere* sphere = (struct CollisionSphere*)typeData->data; + return (2.0f / 5.0f) * mass * sphere->radius * sphere->radius; +} + +struct ColliderCallbacks gCollisionSphereCallbacks = { + collisionSphereCollidePlane, + collisionSphereCollideQuad, + collisionSphereSolidMofI, +}; \ No newline at end of file diff --git a/src/physics/collision_sphere.h b/src/physics/collision_sphere.h new file mode 100644 index 0000000..fa00131 --- /dev/null +++ b/src/physics/collision_sphere.h @@ -0,0 +1,17 @@ +#ifndef __COLLISION_SPHERE_H__ +#define __COLLISION_SPHERE_H__ + +#include "collision.h" + +extern struct ColliderCallbacks gCollisionSphereCallbacks; + +struct CollisionSphere { + float radius; +}; + +int collisionSphereCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactConstraintState* output); +int collisionSphereCollidePlane(void* data, struct Transform* boxTransform, struct Plane* plane, struct ContactConstraintState* contact); +float collisionSphereSolidMofI(struct ColliderTypeData* typeData, float mass); + + +#endif \ No newline at end of file diff --git a/src/scene/cube.c b/src/scene/cube.c index b6a6cba..4ae0601 100644 --- a/src/scene/cube.c +++ b/src/scene/cube.c @@ -4,6 +4,7 @@ #include "defs.h" #include "../graphics/debug_render.h" #include "levels/levels.h" +#include "physics/collision_scene.h" struct CollisionBox gCubeCollisionBox = { {0.3165f, 0.3165f, 0.3165f} @@ -64,12 +65,7 @@ void cubeUpdate(struct Cube* cube) { // collisionObjectCollideWithPlane(&cube->collisionObject, &gFloorObject, &gContactSolver); // collisionObjectCollideWithQuad(&cube->collisionObject, &gFloatingQuadObject, &gContactSolver); - int colliderCount = levelsGetCollisionObjectCount(); - struct CollisionObject* objects = levelsGetCollisionObjects(); - - for (int i = 0; i < colliderCount; ++i) { - collisionObjectCollideWithQuad(&cube->collisionObject, &objects[i], &gContactSolver); - } + collisionObjectCollideWithScene(&cube->collisionObject, &gCollisionScene, &gContactSolver); contactSolverSolve(&gContactSolver); rigidBodyUpdate(&cube->rigidBody);