src/bz/collision/collision.c

   1 #include <bz/collision/collision.h>
   2
   3 #include <bz/debug/assert.h>
   4 #include <bz/math/math.h>
   5 #include <bz/memory/allocator.h>
   6
   7 struct BZCollisionBody {
   8         BZCollisionBodyType type;
   9         BZCollisionShape shape;
  10         BZIdentifierHash tagHash;
  11         void *userParameter;
  12 };
  13
  14 struct BZCollisionSpace {
  15         size_t maxBodies;
  16         size_t nextBody;
  17         BZCollisionBody bodies[];
  18 };
  19
  20 const BZCollisionShapeType BZCollisionShapeTypeMax = BZCollisionShapeTypeLine;
  21
  22 enum CollisionTestResult {
  23     CollisionTestResultNoCollision,
  24     CollisionTestResultCollision,
  25     CollisionTestResultCollisionInverted,
  26 };
  27 typedef enum CollisionTestResult CollisionTestResult;
  28
  29 struct CollisionDetails {
  30         BZVector position;
  31         BZVector normal;
  32 };
  33 typedef struct CollisionDetails CollisionDetails;
  34
  35 static bool collisionCircleCircleTest(CollisionDetails *detailsOut, float *penetrationOut, const BZCollisionShape *shape1, const BZCollisionShape *shape2) {
  36         bool colliding = false;
  37
  38         BZVector delta;
  39         bzVectorSubtract(&delta, &shape2->circleOrigin, &shape1->circleOrigin);
  40         float radius = shape1->circleRadius + shape2->circleRadius;
  41
  42         float deltaMagnitude = bzVectorMagnitude(&delta);
  43         colliding = (deltaMagnitude > 0.01f && deltaMagnitude <= radius);
  44         if (colliding) {
  45                 bzVectorInverse(&delta, &delta);
  46                 bzVectorNormalized(&detailsOut->normal, &delta);
  47                 bzVectorScale(&detailsOut->position, &detailsOut->normal, radius);
  48                 bzVectorAdd(&detailsOut->position, &detailsOut->position, &shape2->circleOrigin);
  49                 *penetrationOut = (radius - deltaMagnitude);
  50         }
  51
  52         return colliding;
  53 }
  54
  55 static bool collisionCircleCirclePerimeterTest(CollisionDetails *detailsOut, float *penetrationOut, const BZCollisionShape *shape1, const BZCollisionShape *shape2) {
  56         bool colliding = false;
  57
  58         BZVector delta;
  59         bzVectorSubtract(&delta, &shape1->circleOrigin, &shape2->circleOrigin);
  60         float innerRadius = shape2->circleRadius - shape1->circleRadius;
  61
  62         float deltaMagnitude = bzVectorMagnitude(&delta);
  63         colliding = deltaMagnitude > innerRadius;// (deltaMagnitude > 0.01f && deltaMagnitude <= radius);
  64         if (colliding) {
  65                 bzVectorNormalized(&detailsOut->normal, &delta);
  66                 bzVectorScale(&detailsOut->position, &detailsOut->normal, shape2->circleRadius);
  67                 bzVectorAdd(&detailsOut->position, &detailsOut->position, &shape2->circleOrigin);
  68
  69                 bzVectorInverse(&detailsOut->normal, &detailsOut->normal);
  70
  71                 *penetrationOut = shape1->circleRadius;
  72         }
  73
  74         return colliding;
  75 }
  76
  77 static bool collisionLineLineTest(CollisionDetails *detailsOut, float *penetrationOut, const BZCollisionShape *shape1, const BZCollisionShape *shape2) {
  78         bool colliding = false;
  79
  80         BZVector delta;
  81         bzVectorSubtract(&delta, &shape2->lineOrigin, &shape1->lineOrigin);
  82
  83         float lineCross = bzVectorCross(&shape1->lineDirection, &shape2->lineDirection);
  84
  85         float t = bzVectorCross(&delta, &shape2->lineDirection) / lineCross;
  86         if (t >= 0.0f && t <= shape1->lineMagnitude) {
  87                 float u = bzVectorCross(&delta, &shape1->lineDirection) / lineCross;
  88                 if (u >= 0.0f && u <= shape2->lineMagnitude) {
  89                         bzVectorScale(&delta, &shape1->lineDirection, t);
  90                         bzVectorAdd(&detailsOut->position, &delta, &shape1->lineOrigin);
  91                         colliding = true;
  92                 }
  93         }
  94
  95         return colliding;
  96 }
  97
  98 static bool collisionCircleLineTest(CollisionDetails *detailsOut, float *penetrationOut, const BZCollisionShape *shape1, const BZCollisionShape *shape2) {
  99         bool colliding = false;
 100
 101         BZVector delta;
 102         bzVectorSubtract(&delta, &shape1->circleOrigin, &shape2->lineOrigin);
 103
 104         float dot = bzVectorDot(&delta, &shape2->lineDirection);
 105         if (dot > 0.01f && dot <= shape2->lineMagnitude) {
 106                 BZVector projection, test;
 107                 bzVectorScale(&projection, &shape2->lineDirection, dot);
 108                 bzVectorSubtract(&test, &delta, &projection);
 109                 float magnitude = bzVectorMagnitude(&test);
 110                 colliding = (magnitude > 0.01f && magnitude <= shape1->circleRadius);
 111                 if (colliding) {
 112                         bzVectorAdd(&detailsOut->position, &projection, &shape2->lineOrigin);
 113                         bzVectorNormalized(&detailsOut->normal, &test);
 114                         *penetrationOut = bzAbs(magnitude - shape1->circleRadius);
 115                 }
 116         }
 117
 118         return colliding;
 119 }
 120
 121
 122
 123 static bool collisionCircleTriangleTest(CollisionDetails *detailsOut, float *penetrationOut, const BZCollisionShape *shape1, const BZCollisionShape *shape2) {
 124         // https://stackoverflow.com/questions/2049582/how-to-determine-if-a-point-is-in-a-2d-triangle
 125
 126         #define tsign(x1, y1, x2, y2, x3, y3) (((x1)-(x3))*((y2)-(y3))-((x2)-(x3))*((y1)-(y3)))
 127
 128         float x = shape1->circleOrigin.x;
 129         float y = shape1->circleOrigin.y;
 130
 131         float x1 = shape2->trianglePoint1.x;
 132         float y1 = shape2->trianglePoint1.y;
 133         float x2 = shape2->trianglePoint2.x;
 134         float y2 = shape2->trianglePoint2.y;
 135         float x3 = shape2->trianglePoint3.x;
 136         float y3 = shape2->trianglePoint3.y;
 137
 138         float a = tsign(x,y,x1,y1,x2,y2);
 139         float b = tsign(x,y,x2,y2,x3,y3);
 140         float c = tsign(x,y,x3,y3,x1,y1);
 141
 142         // FIXME, need to do radius.
 143
 144         return ! ((a<0 || b<0 || c<0) && (a>0 || b>0 || c>0));
 145
 146         /*bool colliding = false;
 147
 148         BZVector delta;
 149         bzVectorSubtract(&delta, &shape1->circleOrigin, &shape2->lineOrigin);
 150
 151         float dot = bzVectorDot(&delta, &shape2->lineDirection);
 152         if (dot > 0.01f && dot <= shape2->lineMagnitude) {
 153                 BZVector projection, test;
 154                 bzVectorScale(&projection, &shape2->lineDirection, dot);
 155                 bzVectorSubtract(&test, &delta, &projection);
 156                 float magnitude = bzVectorMagnitude(&test);
 157                 colliding = (magnitude > 0.01f && magnitude <= shape1->circleRadius);
 158                 if (colliding) {
 159                         bzVectorAdd(&detailsOut->position, &projection, &shape2->lineOrigin);
 160                         bzVectorNormalized(&detailsOut->normal, &test);
 161                         *penetrationOut = bzAbs(magnitude - shape1->circleRadius);
 162                 }
 163         }
 164
 165         return colliding;*/
 166         return false;
 167 }
 168
 169 static CollisionTestResult collisionTest(CollisionDetails *detailsOut, float *penetrationOut, const BZCollisionShape *shape1, const BZCollisionShape *shape2) {
 170         #define TEST_MASK(type1, type2) (((type1) << BZCollisionShapeTypeMax) | (type2))
 171
 172         switch (TEST_MASK(shape1->type, shape2->type)) {
 173                 case TEST_MASK(BZCollisionShapeTypeCircle, BZCollisionShapeTypeCircle):
 174                         return collisionCircleCircleTest(detailsOut, penetrationOut, shape1, shape2) ? CollisionTestResultCollision : CollisionTestResultNoCollision;
 175
 176                 case TEST_MASK(BZCollisionShapeTypeCircle, BZCollisionShapeTypeCirclePerimeter):
 177                         return collisionCircleCirclePerimeterTest(detailsOut, penetrationOut, shape1, shape2) ? CollisionTestResultCollision : CollisionTestResultNoCollision;
 178
 179                 case TEST_MASK(BZCollisionShapeTypeCirclePerimeter, BZCollisionShapeTypeCircle):
 180                         return collisionCircleCirclePerimeterTest(detailsOut, penetrationOut, shape2, shape1) ? CollisionTestResultCollisionInverted : CollisionTestResultNoCollision;
 181
 182                 case TEST_MASK(BZCollisionShapeTypeLine, BZCollisionShapeTypeLine):
 183                         return collisionLineLineTest(detailsOut, penetrationOut, shape1, shape2) ? CollisionTestResultCollision : CollisionTestResultNoCollision;
 184
 185                 case TEST_MASK(BZCollisionShapeTypeCircle, BZCollisionShapeTypeLine):
 186                         return collisionCircleLineTest(detailsOut, penetrationOut, shape1, shape2) ? CollisionTestResultCollision : CollisionTestResultNoCollision;
 187
 188                 case TEST_MASK(BZCollisionShapeTypeLine, BZCollisionShapeTypeCircle):
 189                         return collisionCircleLineTest(detailsOut, penetrationOut, shape2, shape1) ? CollisionTestResultCollisionInverted : CollisionTestResultNoCollision;
 190
 191                 case TEST_MASK(BZCollisionShapeTypeCircle, BZCollisionShapeTypeTriangle):
 192                         return collisionCircleTriangleTest(detailsOut, penetrationOut, shape1, shape2) ? CollisionTestResultCollision : CollisionTestResultNoCollision;
 193
 194                 case TEST_MASK(BZCollisionShapeTypeTriangle, BZCollisionShapeTypeCircle):
 195                         return collisionCircleTriangleTest(detailsOut, penetrationOut, shape2, shape1) ? CollisionTestResultCollisionInverted : CollisionTestResultNoCollision;
 196
 197
 198                 default:
 199                         bzAssertMessage(false, "Invalid collision test");
 200                         return 0;
 201         }
 202 }
 203
 204
 205
 206
 207
 208
 209
 210
 211
 212
 213
 214
 215
 216 BZCollisionSpaceID bzCollisionMakeSpace(BZMemoryArenaID arena, size_t maxBodies, const char *identifierFmt, ...) {
 217         BZCollisionSpaceID space = (BZCollisionSpace *)bzMemoryAlloc(arena, sizeof(BZCollisionSpace) + maxBodies * sizeof(BZCollisionBody));
 218         space->maxBodies = maxBodies;
 219         bzCollisionResetSpace(space);
 220         return space;
 221 }
 222
 223 void bzCollisionResetSpace(BZCollisionSpaceID space) {
 224         space->nextBody = 0;
 225 }
 226
 227 BZCollisionBodyID bzCollisionAddBody(BZCollisionSpaceID space, BZCollisionBodyType bodyType, const BZCollisionShape *shape, BZIdentifierHash tagHash, void *userParameter) {
 228         bzAssert(space->nextBody < space->maxBodies);
 229         BZCollisionBodyID body = &space->bodies[space->nextBody++];
 230         body->type = bodyType;
 231         body->shape = *shape;
 232         body->tagHash = tagHash;
 233         body->userParameter = userParameter;
 234
 235         return body;
 236 }
 237
 238 bool bzCollisionResolve(BZVector *positionOut, void **userParameterOut, BZCollisionSpaceID space, const BZCollisionShape *shape, BZIdentifierHash tagHash) {
 239         CollisionDetails details;
 240         float penetration;
 241         for (size_t i = 0; i < space->nextBody; ++i) {
 242                 BZCollisionBody *body = &space->bodies[i];
 243                 if (body->tagHash == tagHash) {
 244                         CollisionTestResult testResult = collisionTest(&details, &penetration, shape, &body->shape);
 245                         if (testResult != CollisionTestResultNoCollision) {
 246                                 if (positionOut != NULL) {
 247                                         BZVector delta;
 248                                         bzVectorScale(&delta, &details.normal, penetration);
 249                                         bzVectorAdd(positionOut, &details.position, &delta);
 250                                 }
 251                                 if (userParameterOut != NULL) {
 252                                         *userParameterOut = body->userParameter;
 253                                 }
 254                                 return true;
 255                         }
 256                 }
 257         }
 258
 259         return false;
 260 }
 261
 262
 263
 264
 265 #include <bz/gfx/gfx.h>
 266 void bzCollisionDrawDebug(BZCollisionSpaceID space) {
 267         for (size_t i = 0; i < space->nextBody; ++i) {
 268                 switch (space->bodies[i].shape.type) {
 269                         case BZCollisionShapeTypeCircle:
 270                                 bzCirc(space->bodies[i].shape.circleOrigin.x, space->bodies[i].shape.circleOrigin.y, space->bodies[i].shape.circleRadius, 2);
 271                                 break;
 272
 273                         default:
 274                                 break;
 275                 }
 276         }
 277 }