2 ------------------------------------------------------------------------------
3 Licensing information can be found at the end of the file.
4 ------------------------------------------------------------------------------
8 To create implementation (the function definitions)
9 #define CUTE_C2_IMPLEMENTATION
10 in *one* C/CPP file (translation unit) that includes this file
15 cute_c2 is a single-file header that implements 2D collision detection routines
16 that test for overlap, and optionally can find the collision manifold. The
17 manifold contains all necessary information to prevent shapes from inter-
18 penetrating, which is useful for character controllers, general physics
19 simulation, and user-interface programming.
21 This header implements a group of "immediate mode" functions that should be
22 very easily adapted into pre-existing projects.
27 Most of the math types in this header are for internal use. Users care about
28 the shape types and the collision functions.
37 COLLISION FUNCTIONS (*** is a shape name from the above list):
38 * c2***to*** - boolean YES/NO hittest
39 * c2***to***Manifold - construct manifold to describe how shapes hit
40 * c2GJK - runs GJK algorithm to find closest point pair
42 * c2TOI - computes the time of impact between two shapes, useful for
43 sweeping shapes, or doing shape casts
44 * c2MakePoly - Runs convex hull algorithm and computes normals on input point-set
45 * c2Collided - generic version of c2***to*** funcs
46 * c2Collide - generic version of c2***to***Manifold funcs
47 * c2CastRay - generic version of c2Rayto*** funcs
49 The rest of the header is more or less for internal use. Here is an example of
50 making some shapes and testing for collision:
57 cap.a = first_endpoint;
58 cap.b = second_endpoint;
61 int hit = c2CircletoCapsule(c, cap);
64 handle collision here...
67 For more code examples and tests please see:
68 https://github.com/RandyGaul/cute_header/tree/master/examples_cute_gl_and_c2
70 Here is a past discussion thread on this header:
71 https://www.reddit.com/r/gamedev/comments/5tqyey/tinyc2_2d_collision_detection_library_in_c/
73 Here is a very nice repo containing various tests and examples using SFML for rendering:
74 https://github.com/sro5h/tinyc2-tests
79 This header does not implement a broad-phase, and instead concerns itself with
80 the narrow-phase. This means this header just checks to see if two individual
81 shapes are touching, and can give information about how they are touching.
83 Very common 2D broad-phases are tree and grid approaches. Quad trees are good
84 for static geometry that does not move much if at all. Dynamic AABB trees are
85 good for general purpose use, and can handle moving objects very well. Grids
86 are great and are similar to quad trees.
88 If implementing a grid it can be wise to have each collideable grid cell hold
89 an integer. This integer refers to a 2D shape that can be passed into the
90 various functions in this header. The shape can be transformed from "model"
91 space to "world" space using c2x -- a transform struct. In this way a grid
92 can be implemented that holds any kind of convex shape (that this header
93 supports) while conserving memory with shape instancing.
95 Please email at my address with any questions or comments at:
96 author's last name followed by 1748 at gmail
101 * Circles, capsules, AABBs, rays and convex polygons are supported
102 * Fast boolean only result functions (hit yes/no)
103 * Slghtly slower manifold generation for collision normals + depths +points
104 * GJK implementation (finds closest points for disjoint pairs of shapes)
105 * Shape casts/sweeps with c2TOI function (time of impact)
106 * Robust 2D convex hull generator
107 * Lots of correctly implemented and tested 2D math routines
108 * Implemented in portable C, and is readily portable to other languages
109 * Generic c2Collide, c2Collided and c2CastRay function (can pass in any shape type)
110 * Extensive examples at: https://github.com/RandyGaul/cute_headers/tree/master/examples_cute_gl_and_c2
115 1.0 (02/13/2017) initial release
116 1.01 (02/13/2017) const crusade, minor optimizations, capsule degen
117 1.02 (03/21/2017) compile fixes for c on more compilers
118 1.03 (09/15/2017) various bugfixes and quality of life changes to manifolds
119 1.04 (03/25/2018) fixed manifold bug in c2CircletoAABBManifold
120 1.05 (11/01/2018) added c2TOI (time of impact) for shape cast/sweep test
121 1.06 (08/23/2019) C2_*** types to C2_TYPE_***, and CUTE_C2_API
126 Plastburk 1.01 - const pointers pull request
127 mmozeiko 1.02 - 3 compile bugfixes
128 felipefs 1.02 - 3 compile bugfixes
129 seemk 1.02 - fix branching bug in c2Collide
130 sro5h 1.02 - bug reports for multiple manifold funcs
131 sro5h 1.03 - work involving quality of life fixes for manifolds
132 Wizzard033 1.06 - C2_*** types to C2_TYPE_***, and CUTE_C2_API
135 #if !defined(CUTE_C2_H)
137 // this can be adjusted as necessary, but is highly recommended to be kept at 8.
138 // higher numbers will incur quite a bit of memory overhead, and convex shapes
139 // over 8 verts start to just look like spheres, which can be implicitly rep-
140 // resented as a point + radius. usually tools that generate polygons should be
141 // constructed so they do not output polygons with too many verts.
142 // Note: polygons in cute_c2 are all *convex*.
143 #define C2_MAX_POLYGON_VERTS 8
152 // 2d rotation composed of cos/sin pair
159 // 2d rotation matrix
166 // 2d transformation "x"
167 // These are used especially for c2Poly when a c2Poly is passed to a function.
168 // Since polygons are prime for "instancing" a c2x transform can be used to
169 // transform a polygon from local space to world space. In functions that take
170 // a c2x pointer (like c2PolytoPoly), these pointers can be NULL, which represents
171 // an identity transformation and assumes the verts inside of c2Poly are already
179 // 2d halfspace (aka plane, aka line)
182 c2v n; // normal, normalized
183 float d; // distance to origin from plane, or ax + by = d
198 // a capsule is defined as a line segment (from a to b) and radius r
209 c2v verts[C2_MAX_POLYGON_VERTS];
210 c2v norms[C2_MAX_POLYGON_VERTS];
214 // Many algorithms in this file are sensitive to the magnitude of the
215 // ray direction (c2Ray::d). It is highly recommended to normalize the
216 // ray direction and use t to specify a distance. Please see this link
217 // for an in-depth explanation: https://github.com/RandyGaul/cute_headers/issues/30
221 c2v d; // direction (normalized)
222 float t; // distance along d from position p to find endpoint of ray
227 float t; // time of impact
228 c2v n; // normal of surface at impact (unit length)
231 // position of impact p = ray.p + ray.d * raycast.t
232 #define c2Impact(ray, t) c2Add(ray.p, c2Mulvs(ray.d, t))
234 // contains all information necessary to resolve a collision, or in other words
235 // this is the information needed to separate shapes that are colliding. Doing
236 // the resolution step is *not* included in cute_c2. cute_c2 does not include
237 // "feature information" that describes which topological features collided.
238 // However, modifying the exist ***Manifold funcs can be done to output any
239 // needed feature information. Feature info is sometimes needed for certain kinds
240 // of simulations that cache information over multiple game-ticks, of which are
241 // associated to the collision of specific features. An example implementation
242 // is in the qu3e 3D physics engine library: https://github.com/RandyGaul/qu3e
247 c2v contact_points[2];
249 // always points from shape A to shape B (first and second shapes passed into
250 // any of the c2***to***Manifold functions)
254 // This define allows exporting/importing of the header to a dynamic library.
255 // Here's an example.
256 // #define CUTE_C2_API extern "C" __declspec(dllexport)
257 #if !defined(CUTE_C2_API)
261 // boolean collision detection
262 // these versions are faster than the manifold versions, but only give a YES/NO
264 CUTE_C2_API int c2CircletoCircle(c2Circle A, c2Circle B);
265 CUTE_C2_API int c2CircletoAABB(c2Circle A, c2AABB B);
266 CUTE_C2_API int c2CircletoCapsule(c2Circle A, c2Capsule B);
267 CUTE_C2_API int c2AABBtoAABB(c2AABB A, c2AABB B);
268 CUTE_C2_API int c2AABBtoCapsule(c2AABB A, c2Capsule B);
269 CUTE_C2_API int c2CapsuletoCapsule(c2Capsule A, c2Capsule B);
270 CUTE_C2_API int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx);
271 CUTE_C2_API int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx);
272 CUTE_C2_API int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx);
273 CUTE_C2_API int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx);
276 // output is placed into the c2Raycast struct, which represents the hit location
277 // of the ray. the out param contains no meaningful information if these funcs
279 CUTE_C2_API int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out);
280 CUTE_C2_API int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out);
281 CUTE_C2_API int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out);
282 CUTE_C2_API int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out);
284 // manifold generation
285 // these functions are slower than the boolean versions, but will compute one
286 // or two points that represent the plane of contact. This information is
287 // is usually needed to resolve and prevent shapes from colliding. If no coll
288 // ision occured the count member of the manifold struct is set to 0.
289 CUTE_C2_API void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m);
290 CUTE_C2_API void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m);
291 CUTE_C2_API void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m);
292 CUTE_C2_API void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m);
293 CUTE_C2_API void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m);
294 CUTE_C2_API void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m);
295 CUTE_C2_API void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx, c2Manifold* m);
296 CUTE_C2_API void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m);
297 CUTE_C2_API void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx, c2Manifold* m);
298 CUTE_C2_API void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx, c2Manifold* m);
309 // This struct is only for advanced usage of the c2GJK function. See comments inside of the
310 // c2GJK function for more details.
320 // Runs the GJK algorithm to find closest points, returns distance between closest points.
321 // outA and outB can be NULL, in this case only distance is returned. ax_ptr and bx_ptr
322 // can be NULL, and represent local to world transformations for shapes A and B respectively.
323 // use_radius will apply radii for capsules and circles (if set to false, spheres are
324 // treated as points and capsules are treated as line segments i.e. rays). The cache parameter
325 // should be NULL, as it is only for advanced usage (unless you know what you're doing, then
326 // go ahead and use it). iterations is an optional parameter.
327 CUTE_C2_API float c2GJK(const void* A, C2_TYPE typeA, const c2x* ax_ptr, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v* outA, c2v* outB, int use_radius, int* iterations, c2GJKCache* cache);
329 // Computes the time of impact from shape A and shape B. The velocity of each shape is provided
330 // by vA and vB respectively. The shapes are *not* allowed to rotate over time. The velocity is
331 // assumed to represent the change in motion from time 0 to time 1, and so the return value will
332 // be a number from 0 to 1. To move each shape to the colliding configuration, multiply vA and vB
333 // each by the return value. ax_ptr and bx_ptr are optional parameters to transforms for each shape,
334 // and are typically used for polygon shapes to transform from model to world space. Set these to
335 // NULL to represent identity transforms. The out_normal for non-colliding configurations (or in
336 // other words, when the return value is 1) is just the direction pointing along the closest points
337 // from shape A to shape B. out_normal can be NULL. iterations is an optional parameter. use_radius
338 // will apply radii for capsules and circles (if set to false, spheres are treated as points and
339 // capsules are treated as line segments i.e. rays).
340 CUTE_C2_API float c2TOI(const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, int use_radius, c2v* out_normal, c2v* out_contact_point, int* iterations);
342 // Computes 2D convex hull. Will not do anything if less than two verts supplied. If
343 // more than C2_MAX_POLYGON_VERTS are supplied extras are ignored.
344 CUTE_C2_API int c2Hull(c2v* verts, int count);
345 CUTE_C2_API void c2Norms(c2v* verts, c2v* norms, int count);
347 // runs c2Hull and c2Norms, assumes p->verts and p->count are both set to valid values
348 CUTE_C2_API void c2MakePoly(c2Poly* p);
350 // Generic collision detection routines, useful for games that want to use some poly-
351 // morphism to write more generic-styled code. Internally calls various above functions.
352 // For AABBs/Circles/Capsules ax and bx are ignored. For polys ax and bx can define
353 // model to world transformations, or be NULL for identity transforms.
354 CUTE_C2_API int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB);
355 CUTE_C2_API void c2Collide(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB, c2Manifold* m);
356 CUTE_C2_API int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out);
359 #define C2_INLINE __forceinline
361 #define C2_INLINE inline __attribute__((always_inline))
364 // adjust these primitives as seen fit
365 #include <string.h> // memcpy
367 #define c2Sin(radians) sinf(radians)
368 #define c2Cos(radians) cosf(radians)
369 #define c2Sqrt(a) sqrtf(a)
370 #define c2Min(a, b) ((a) < (b) ? (a) : (b))
371 #define c2Max(a, b) ((a) > (b) ? (a) : (b))
372 #define c2Abs(a) ((a) < 0 ? -(a) : (a))
373 #define c2Clamp(a, lo, hi) c2Max(lo, c2Min(a, hi))
374 C2_INLINE void c2SinCos(float radians, float* s, float* c) { *c = c2Cos(radians); *s = c2Sin(radians); }
375 #define c2Sign(a) (a < 0 ? -1.0f : 1.0f)
377 // The rest of the functions in the header-only portion are all for internal use
378 // and use the author's personal naming conventions. It is recommended to use one's
379 // own math library instead of the one embedded here in cute_c2, but for those
380 // curious or interested in trying it out here's the details:
382 // The Mul functions are used to perform multiplication. x stands for transform,
383 // v stands for vector, s stands for scalar, r stands for rotation, h stands for
384 // halfspace and T stands for transpose.For example c2MulxvT stands for "multiply
385 // a transform with a vector, and transpose the transform".
388 C2_INLINE c2v c2V(float x, float y) { c2v a; a.x = x; a.y = y; return a; }
389 C2_INLINE c2v c2Add(c2v a, c2v b) { a.x += b.x; a.y += b.y; return a; }
390 C2_INLINE c2v c2Sub(c2v a, c2v b) { a.x -= b.x; a.y -= b.y; return a; }
391 C2_INLINE float c2Dot(c2v a, c2v b) { return a.x * b.x + a.y * b.y; }
392 C2_INLINE c2v c2Mulvs(c2v a, float b) { a.x *= b; a.y *= b; return a; }
393 C2_INLINE c2v c2Mulvv(c2v a, c2v b) { a.x *= b.x; a.y *= b.y; return a; }
394 C2_INLINE c2v c2Div(c2v a, float b) { return c2Mulvs(a, 1.0f / b); }
395 C2_INLINE c2v c2Skew(c2v a) { c2v b; b.x = -a.y; b.y = a.x; return b; }
396 C2_INLINE c2v c2CCW90(c2v a) { c2v b; b.x = a.y; b.y = -a.x; return b; }
397 C2_INLINE float c2Det2(c2v a, c2v b) { return a.x * b.y - a.y * b.x; }
398 C2_INLINE c2v c2Minv(c2v a, c2v b) { return c2V(c2Min(a.x, b.x), c2Min(a.y, b.y)); }
399 C2_INLINE c2v c2Maxv(c2v a, c2v b) { return c2V(c2Max(a.x, b.x), c2Max(a.y, b.y)); }
400 C2_INLINE c2v c2Clampv(c2v a, c2v lo, c2v hi) { return c2Maxv(lo, c2Minv(a, hi)); }
401 C2_INLINE c2v c2Absv(c2v a) { return c2V(c2Abs(a.x), c2Abs(a.y)); }
402 C2_INLINE float c2Hmin(c2v a) { return c2Min(a.x, a.y); }
403 C2_INLINE float c2Hmax(c2v a) { return c2Max(a.x, a.y); }
404 C2_INLINE float c2Len(c2v a) { return c2Sqrt(c2Dot(a, a)); }
405 C2_INLINE c2v c2Norm(c2v a) { return c2Div(a, c2Len(a)); }
406 C2_INLINE c2v c2SafeNorm(c2v a) { float sq = c2Dot(a, a); return sq ? c2Div(a, c2Len(a)) : c2V(0, 0); }
407 C2_INLINE c2v c2Neg(c2v a) { return c2V(-a.x, -a.y); }
408 C2_INLINE c2v c2Lerp(c2v a, c2v b, float t) { return c2Add(a, c2Mulvs(c2Sub(b, a), t)); }
409 C2_INLINE int c2Parallel(c2v a, c2v b, float kTol)
411 float k = c2Len(a) / c2Len(b);
413 if (c2Abs(a.x - b.x) < kTol && c2Abs(a.y - b.y) < kTol) return 1;
418 C2_INLINE c2r c2Rot(float radians) { c2r r; c2SinCos(radians, &r.s, &r.c); return r; }
419 C2_INLINE c2r c2RotIdentity() { c2r r; r.c = 1.0f; r.s = 0; return r; }
420 C2_INLINE c2v c2RotX(c2r r) { return c2V(r.c, r.s); }
421 C2_INLINE c2v c2RotY(c2r r) { return c2V(-r.s, r.c); }
422 C2_INLINE c2v c2Mulrv(c2r a, c2v b) { return c2V(a.c * b.x - a.s * b.y, a.s * b.x + a.c * b.y); }
423 C2_INLINE c2v c2MulrvT(c2r a, c2v b) { return c2V(a.c * b.x + a.s * b.y, -a.s * b.x + a.c * b.y); }
424 C2_INLINE c2r c2Mulrr(c2r a, c2r b) { c2r c; c.c = a.c * b.c - a.s * b.s; c.s = a.s * b.c + a.c * b.s; return c; }
425 C2_INLINE c2r c2MulrrT(c2r a, c2r b) { c2r c; c.c = a.c * b.c + a.s * b.s; c.s = a.c * b.s - a.s * b.c; return c; }
427 C2_INLINE c2v c2Mulmv(c2m a, c2v b) { c2v c; c.x = a.x.x * b.x + a.y.x * b.y; c.y = a.x.y * b.x + a.y.y * b.y; return c; }
428 C2_INLINE c2v c2MulmvT(c2m a, c2v b) { c2v c; c.x = a.x.x * b.x + a.x.y * b.y; c.y = a.y.x * b.x + a.y.y * b.y; return c; }
429 C2_INLINE c2m c2Mulmm(c2m a, c2m b) { c2m c; c.x = c2Mulmv(a, b.x); c.y = c2Mulmv(a, b.y); return c; }
430 C2_INLINE c2m c2MulmmT(c2m a, c2m b) { c2m c; c.x = c2MulmvT(a, b.x); c.y = c2MulmvT(a, b.y); return c; }
433 C2_INLINE c2x c2xIdentity() { c2x x; x.p = c2V(0, 0); x.r = c2RotIdentity(); return x; }
434 C2_INLINE c2v c2Mulxv(c2x a, c2v b) { return c2Add(c2Mulrv(a.r, b), a.p); }
435 C2_INLINE c2v c2MulxvT(c2x a, c2v b) { return c2MulrvT(a.r, c2Sub(b, a.p)); }
436 C2_INLINE c2x c2Mulxx(c2x a, c2x b) { c2x c; c.r = c2Mulrr(a.r, b.r); c.p = c2Add(c2Mulrv(a.r, b.p), a.p); return c; }
437 C2_INLINE c2x c2MulxxT(c2x a, c2x b) { c2x c; c.r = c2MulrrT(a.r, b.r); c.p = c2MulrvT(a.r, c2Sub(b.p, a.p)); return c; }
438 C2_INLINE c2x c2Transform(c2v p, float radians) { c2x x; x.r = c2Rot(radians); x.p = p; return x; }
441 C2_INLINE c2v c2Origin(c2h h) { return c2Mulvs(h.n, h.d); }
442 C2_INLINE float c2Dist(c2h h, c2v p) { return c2Dot(h.n, p) - h.d; }
443 C2_INLINE c2v c2Project(c2h h, c2v p) { return c2Sub(p, c2Mulvs(h.n, c2Dist(h, p))); }
444 C2_INLINE c2h c2Mulxh(c2x a, c2h b) { c2h c; c.n = c2Mulrv(a.r, b.n); c.d = c2Dot(c2Mulxv(a, c2Origin(b)), c.n); return c; }
445 C2_INLINE c2h c2MulxhT(c2x a, c2h b) { c2h c; c.n = c2MulrvT(a.r, b.n); c.d = c2Dot(c2MulxvT(a, c2Origin(b)), c.n); return c; }
446 C2_INLINE c2v c2Intersect(c2v a, c2v b, float da, float db) { return c2Add(a, c2Mulvs(c2Sub(b, a), (da / (da - db)))); }
448 C2_INLINE void c2BBVerts(c2v* out, c2AABB* bb)
451 out[1] = c2V(bb->max.x, bb->min.y);
453 out[3] = c2V(bb->min.x, bb->max.y);
459 #ifdef CUTE_C2_IMPLEMENTATION
460 #ifndef CUTE_C2_IMPLEMENTATION_ONCE
461 #define CUTE_C2_IMPLEMENTATION_ONCE
463 int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB)
470 case C2_TYPE_CIRCLE: return c2CircletoCircle(*(c2Circle*)A, *(c2Circle*)B);
471 case C2_TYPE_AABB: return c2CircletoAABB(*(c2Circle*)A, *(c2AABB*)B);
472 case C2_TYPE_CAPSULE: return c2CircletoCapsule(*(c2Circle*)A, *(c2Capsule*)B);
473 case C2_TYPE_POLY: return c2CircletoPoly(*(c2Circle*)A, (const c2Poly*)B, bx);
481 case C2_TYPE_CIRCLE: return c2CircletoAABB(*(c2Circle*)B, *(c2AABB*)A);
482 case C2_TYPE_AABB: return c2AABBtoAABB(*(c2AABB*)A, *(c2AABB*)B);
483 case C2_TYPE_CAPSULE: return c2AABBtoCapsule(*(c2AABB*)A, *(c2Capsule*)B);
484 case C2_TYPE_POLY: return c2AABBtoPoly(*(c2AABB*)A, (const c2Poly*)B, bx);
489 case C2_TYPE_CAPSULE:
492 case C2_TYPE_CIRCLE: return c2CircletoCapsule(*(c2Circle*)B, *(c2Capsule*)A);
493 case C2_TYPE_AABB: return c2AABBtoCapsule(*(c2AABB*)B, *(c2Capsule*)A);
494 case C2_TYPE_CAPSULE: return c2CapsuletoCapsule(*(c2Capsule*)A, *(c2Capsule*)B);
495 case C2_TYPE_POLY: return c2CapsuletoPoly(*(c2Capsule*)A, (const c2Poly*)B, bx);
503 case C2_TYPE_CIRCLE: return c2CircletoPoly(*(c2Circle*)B, (const c2Poly*)A, ax);
504 case C2_TYPE_AABB: return c2AABBtoPoly(*(c2AABB*)B, (const c2Poly*)A, ax);
505 case C2_TYPE_CAPSULE: return c2CapsuletoPoly(*(c2Capsule*)B, (const c2Poly*)A, ax);
506 case C2_TYPE_POLY: return c2PolytoPoly((const c2Poly*)A, ax, (const c2Poly*)B, bx);
516 void c2Collide(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB, c2Manifold* m)
525 case C2_TYPE_CIRCLE: c2CircletoCircleManifold(*(c2Circle*)A, *(c2Circle*)B, m); break;
526 case C2_TYPE_AABB: c2CircletoAABBManifold(*(c2Circle*)A, *(c2AABB*)B, m); break;
527 case C2_TYPE_CAPSULE: c2CircletoCapsuleManifold(*(c2Circle*)A, *(c2Capsule*)B, m); break;
528 case C2_TYPE_POLY: c2CircletoPolyManifold(*(c2Circle*)A, (const c2Poly*)B, bx, m); break;
535 case C2_TYPE_CIRCLE: c2CircletoAABBManifold(*(c2Circle*)B, *(c2AABB*)A, m); m->n = c2Neg(m->n); break;
536 case C2_TYPE_AABB: c2AABBtoAABBManifold(*(c2AABB*)A, *(c2AABB*)B, m); break;
537 case C2_TYPE_CAPSULE: c2AABBtoCapsuleManifold(*(c2AABB*)A, *(c2Capsule*)B, m); break;
538 case C2_TYPE_POLY: c2AABBtoPolyManifold(*(c2AABB*)A, (const c2Poly*)B, bx, m); break;
542 case C2_TYPE_CAPSULE:
545 case C2_TYPE_CIRCLE: c2CircletoCapsuleManifold(*(c2Circle*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
546 case C2_TYPE_AABB: c2AABBtoCapsuleManifold(*(c2AABB*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
547 case C2_TYPE_CAPSULE: c2CapsuletoCapsuleManifold(*(c2Capsule*)A, *(c2Capsule*)B, m); break;
548 case C2_TYPE_POLY: c2CapsuletoPolyManifold(*(c2Capsule*)A, (const c2Poly*)B, bx, m); break;
555 case C2_TYPE_CIRCLE: c2CircletoPolyManifold(*(c2Circle*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
556 case C2_TYPE_AABB: c2AABBtoPolyManifold(*(c2AABB*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
557 case C2_TYPE_CAPSULE: c2CapsuletoPolyManifold(*(c2Capsule*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
558 case C2_TYPE_POLY: c2PolytoPolyManifold((const c2Poly*)A, ax, (const c2Poly*)B, bx, m); break;
564 int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out)
568 case C2_TYPE_CIRCLE: return c2RaytoCircle(A, *(c2Circle*)B, out);
569 case C2_TYPE_AABB: return c2RaytoAABB(A, *(c2AABB*)B, out);
570 case C2_TYPE_CAPSULE: return c2RaytoCapsule(A, *(c2Capsule*)B, out);
571 case C2_TYPE_POLY: return c2RaytoPoly(A, (const c2Poly*)B, bx, out);
577 #define C2_GJK_ITERS 20
583 c2v verts[C2_MAX_POLYGON_VERTS];
603 static C2_INLINE void c2MakeProxy(const void* shape, C2_TYPE type, c2Proxy* p)
609 c2Circle* c = (c2Circle*)shape;
617 c2AABB* bb = (c2AABB*)shape;
620 c2BBVerts(p->verts, bb);
623 case C2_TYPE_CAPSULE:
625 c2Capsule* c = (c2Capsule*)shape;
634 c2Poly* poly = (c2Poly*)shape;
636 p->count = poly->count;
637 for (int i = 0; i < p->count; ++i) p->verts[i] = poly->verts[i];
642 static C2_INLINE int c2Support(const c2v* verts, int count, c2v d)
645 float dmax = c2Dot(verts[0], d);
647 for (int i = 1; i < count; ++i)
649 float dot = c2Dot(verts[i], d);
660 #define C2_BARY(n, x) c2Mulvs(s->n.x, (den * s->n.u))
661 #define C2_BARY2(x) c2Add(C2_BARY(a, x), C2_BARY(b, x))
662 #define C2_BARY3(x) c2Add(c2Add(C2_BARY(a, x), C2_BARY(b, x)), C2_BARY(c, x))
664 static C2_INLINE c2v c2L(c2Simplex* s)
666 float den = 1.0f / s->div;
669 case 1: return s->a.p;
670 case 2: return C2_BARY2(p);
671 case 3: return C2_BARY3(p);
672 default: return c2V(0, 0);
676 static C2_INLINE void c2Witness(c2Simplex* s, c2v* a, c2v* b)
678 float den = 1.0f / s->div;
681 case 1: *a = s->a.sA; *b = s->a.sB; break;
682 case 2: *a = C2_BARY2(sA); *b = C2_BARY2(sB); break;
683 case 3: *a = C2_BARY3(sA); *b = C2_BARY3(sB); break;
684 default: *a = c2V(0, 0); *b = c2V(0, 0);
688 static C2_INLINE c2v c2D(c2Simplex* s)
692 case 1: return c2Neg(s->a.p);
695 c2v ab = c2Sub(s->b.p, s->a.p);
696 if (c2Det2(ab, c2Neg(s->a.p)) > 0) return c2Skew(ab);
700 default: return c2V(0, 0);
704 static C2_INLINE void c22(c2Simplex* s)
708 float u = c2Dot(b, c2Norm(c2Sub(b, a)));
709 float v = c2Dot(a, c2Norm(c2Sub(a, b)));
735 static C2_INLINE void c23(c2Simplex* s)
741 float uAB = c2Dot(b, c2Norm(c2Sub(b, a)));
742 float vAB = c2Dot(a, c2Norm(c2Sub(a, b)));
743 float uBC = c2Dot(c, c2Norm(c2Sub(c, b)));
744 float vBC = c2Dot(b, c2Norm(c2Sub(b, c)));
745 float uCA = c2Dot(a, c2Norm(c2Sub(a, c)));
746 float vCA = c2Dot(c, c2Norm(c2Sub(c, a)));
747 float area = c2Det2(c2Norm(c2Sub(b, a)), c2Norm(c2Sub(c, a)));
748 float uABC = c2Det2(b, c) * area;
749 float vABC = c2Det2(c, a) * area;
750 float wABC = c2Det2(a, b) * area;
752 if (vAB <= 0 && uCA <= 0)
759 else if (uAB <= 0 && vBC <= 0)
767 else if (uBC <= 0 && vCA <= 0)
775 else if (uAB > 0 && vAB > 0 && wABC <= 0)
783 else if (uBC > 0 && vBC > 0 && uABC <= 0)
793 else if (uCA > 0 && vCA > 0 && vABC <= 0)
808 s->div = uABC + vABC + wABC;
815 static C2_INLINE float c2GJKSimplexMetric(c2Simplex* s)
819 default: // fall through
821 case 2: return c2Len(c2Sub(s->b.p, s->a.p));
822 case 3: return c2Det2(c2Sub(s->b.p, s->a.p), c2Sub(s->c.p, s->a.p));
826 // Please see http://box2d.org/downloads/ under GDC 2010 for Erin's demo code
827 // and PDF slides for documentation on the GJK algorithm. This function is mostly
828 // from Erin's version from his online resources.
829 float c2GJK(const void* A, C2_TYPE typeA, const c2x* ax_ptr, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v* outA, c2v* outB, int use_radius, int* iterations, c2GJKCache* cache)
833 if (!ax_ptr) ax = c2xIdentity();
835 if (!bx_ptr) bx = c2xIdentity();
840 c2MakeProxy(A, typeA, &pA);
841 c2MakeProxy(B, typeB, &pB);
846 // Metric and caching system as designed by E. Catto in Box2D for his conservative advancment/bilateral
847 // advancement algorithim implementations. The purpose is to reuse old simplex indices (any simplex that
848 // have not degenerated into a line or point) as a starting point. This skips the first few iterations of
849 // GJK going from point, to line, to triangle, lowering convergence rates dramatically for temporally
850 // coherent cases (such as in time of impact searches).
851 int cache_was_read = 0;
854 int cache_was_good = !!cache->count;
858 for (int i = 0; i < cache->count; ++i)
860 int iA = cache->iA[i];
861 int iB = cache->iB[i];
862 c2v sA = c2Mulxv(ax, pA.verts[iA]);
863 c2v sB = c2Mulxv(bx, pB.verts[iB]);
869 v->p = c2Sub(v->sB, v->sA);
872 s.count = cache->count;
875 float metric_old = cache->metric;
876 float metric = c2GJKSimplexMetric(&s);
878 float min_metric = metric < metric_old ? metric : metric_old;
879 float max_metric = metric > metric_old ? metric : metric_old;
881 if (!(min_metric < max_metric * 2.0f && metric < -1.0e8f)) cache_was_read = 1;
889 s.a.sA = c2Mulxv(ax, pA.verts[0]);
890 s.a.sB = c2Mulxv(bx, pB.verts[0]);
891 s.a.p = c2Sub(s.a.sB, s.a.sA);
897 int saveA[3], saveB[3];
903 while (iter < C2_GJK_ITERS)
905 save_count = s.count;
906 for (int i = 0; i < save_count; ++i)
908 saveA[i] = verts[i].iA;
909 saveB[i] = verts[i].iB;
915 case 2: c22(&s); break;
916 case 3: c23(&s); break;
932 if (c2Dot(d, d) < FLT_EPSILON * FLT_EPSILON) break;
934 int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(d)));
935 c2v sA = c2Mulxv(ax, pA.verts[iA]);
936 int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, d));
937 c2v sB = c2Mulxv(bx, pB.verts[iB]);
939 c2sv* v = verts + s.count;
944 v->p = c2Sub(v->sB, v->sA);
947 for (int i = 0; i < save_count; ++i)
949 if (iA == saveA[i] && iB == saveB[i])
962 c2Witness(&s, &a, &b);
963 float dist = c2Len(c2Sub(a, b));
973 float rA = pA.radius;
974 float rB = pB.radius;
976 if (dist > rA + rB && dist > FLT_EPSILON)
979 c2v n = c2Norm(c2Sub(b, a));
980 a = c2Add(a, c2Mulvs(n, rA));
981 b = c2Sub(b, c2Mulvs(n, rB));
982 if (a.x == b.x && a.y == b.y) dist = 0;
987 c2v p = c2Mulvs(c2Add(a, b), 0.5f);
996 cache->metric = c2GJKSimplexMetric(&s);
997 cache->count = s.count;
998 for (int i = 0; i < s.count; ++i)
1000 c2sv* v = verts + i;
1001 cache->iA[i] = v->iA;
1002 cache->iB[i] = v->iB;
1007 if (outA) *outA = a;
1008 if (outB) *outB = b;
1009 if (iterations) *iterations = iter;
1013 static C2_INLINE float c2Step(float t, const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, c2v* a, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, c2v* b, int use_radius, c2GJKCache* cache)
1017 ax.p = c2Add(ax.p, c2Mulvs(vA, t));
1018 bx.p = c2Add(bx.p, c2Mulvs(vB, t));
1019 float d = c2GJK(A, typeA, &ax, B, typeB, &bx, a, b, use_radius, NULL, cache);
1023 float c2TOI(const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, int use_radius, c2v* out_normal, c2v* out_contact_point, int* iterations)
1028 if (!ax_ptr) ax = c2xIdentity();
1030 if (!bx_ptr) bx = c2xIdentity();
1035 float d = c2Step(t, A, typeA, &ax, vA, &a, B, typeB, &bx, vB, &b, use_radius, &cache);
1036 c2v v = c2Sub(vB, vA);
1037 n = c2SafeNorm(c2Sub(b, a));
1040 float eps = 1.0e-5f;
1041 while (d > eps && t < 1)
1044 float velocity_bound = c2Abs(c2Dot(c2Norm(c2Sub(b, a)), v));
1045 if (!velocity_bound) return 1;
1046 float delta = d / velocity_bound;
1049 d = c2Step(t, A, typeA, &ax, vA, &a0, B, typeB, &bx, vB, &b0, use_radius, &cache);
1060 c2v p = c2Mulvs(c2Add(a, b), 0.5f);
1062 if (out_normal) *out_normal = n;
1063 if (out_contact_point) *out_contact_point = p;
1064 if (iterations) *iterations = iter;
1068 int c2Hull(c2v* verts, int count)
1070 if (count <= 2) return 0;
1071 count = c2Min(C2_MAX_POLYGON_VERTS, count);
1074 float xmax = verts[0].x;
1075 for (int i = 1; i < count; ++i)
1077 float x = verts[i].x;
1085 if (verts[i].y < verts[right].y) right = i;
1088 int hull[C2_MAX_POLYGON_VERTS];
1094 hull[out_count] = index;
1097 for (int i = 1; i < count; ++i)
1105 c2v e1 = c2Sub(verts[next], verts[hull[out_count]]);
1106 c2v e2 = c2Sub(verts[i], verts[hull[out_count]]);
1107 float c = c2Det2(e1, e2);
1109 if (c == 0 && c2Dot(e2, e2) > c2Dot(e1, e1)) next = i;
1114 if (next == right) break;
1117 c2v hull_verts[C2_MAX_POLYGON_VERTS];
1118 for (int i = 0; i < out_count; ++i) hull_verts[i] = verts[hull[i]];
1119 memcpy(verts, hull_verts, sizeof(c2v) * out_count);
1123 void c2Norms(c2v* verts, c2v* norms, int count)
1125 for (int i = 0; i < count; ++i)
1128 int b = i + 1 < count ? i + 1 : 0;
1129 c2v e = c2Sub(verts[b], verts[a]);
1130 norms[i] = c2Norm(c2CCW90(e));
1134 void c2MakePoly(c2Poly* p)
1136 p->count = c2Hull(p->verts, p->count);
1137 c2Norms(p->verts, p->norms, p->count);
1140 int c2CircletoCircle(c2Circle A, c2Circle B)
1142 c2v c = c2Sub(B.p, A.p);
1143 float d2 = c2Dot(c, c);
1144 float r2 = A.r + B.r;
1149 int c2CircletoAABB(c2Circle A, c2AABB B)
1151 c2v L = c2Clampv(A.p, B.min, B.max);
1152 c2v ab = c2Sub(A.p, L);
1153 float d2 = c2Dot(ab, ab);
1154 float r2 = A.r * A.r;
1158 int c2AABBtoAABB(c2AABB A, c2AABB B)
1160 int d0 = B.max.x < A.min.x;
1161 int d1 = A.max.x < B.min.x;
1162 int d2 = B.max.y < A.min.y;
1163 int d3 = A.max.y < B.min.y;
1164 return !(d0 | d1 | d2 | d3);
1167 // see: http://www.randygaul.net/2014/07/23/distance-point-to-line-segment/
1168 int c2CircletoCapsule(c2Circle A, c2Capsule B)
1170 c2v n = c2Sub(B.b, B.a);
1171 c2v ap = c2Sub(A.p, B.a);
1172 float da = c2Dot(ap, n);
1175 if (da < 0) d2 = c2Dot(ap, ap);
1178 float db = c2Dot(c2Sub(A.p, B.b), n);
1181 c2v e = c2Sub(ap, c2Mulvs(n, (da / c2Dot(n, n))));
1186 c2v bp = c2Sub(A.p, B.b);
1191 float r = A.r + B.r;
1195 int c2AABBtoCapsule(c2AABB A, c2Capsule B)
1197 if (c2GJK(&A, C2_TYPE_AABB, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0;
1201 int c2CapsuletoCapsule(c2Capsule A, c2Capsule B)
1203 if (c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0;
1207 int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx)
1209 if (c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1213 int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx)
1215 if (c2GJK(&A, C2_TYPE_AABB, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1219 int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx)
1221 if (c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1225 int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx)
1227 if (c2GJK(A, C2_TYPE_POLY, ax, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1231 int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out)
1234 c2v m = c2Sub(A.p, p);
1235 float c = c2Dot(m, m) - B.r * B.r;
1236 float b = c2Dot(m, A.d);
1237 float disc = b * b - c;
1238 if (disc < 0) return 0;
1240 float t = -b - c2Sqrt(disc);
1241 if (t >= 0 && t <= A.t)
1244 c2v impact = c2Impact(A, t);
1245 out->n = c2Norm(c2Sub(impact, p));
1251 static inline float c2SignedDistPointToPlane_OneDimensional(float p, float n, float d)
1253 return p * n - d * n;
1256 static inline float c2RayToPlane_OneDimensional(float da, float db)
1258 if (da < 0) return 0; // Ray started behind plane.
1259 else if (da * db >= 0) return 1.0f; // Ray starts and ends on the same of the plane.
1260 else // Ray starts and ends on opposite sides of the plane (or directly on the plane).
1263 if (d != 0) return da / d;
1264 else return 0; // Special case for super tiny ray, or AABB.
1268 int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out)
1271 c2v p1 = c2Impact(A, A.t);
1273 a_box.min = c2Minv(p0, p1);
1274 a_box.max = c2Maxv(p0, p1);
1277 if (!c2AABBtoAABB(a_box, B)) return 0;
1279 // Test the ray's axes (along the segment's normal).
1280 c2v ab = c2Sub(p1, p0);
1282 c2v abs_n = c2Absv(n);
1283 c2v half_extents = c2Mulvs(c2Sub(B.max, B.min), 0.5f);
1284 c2v center_of_b_box = c2Mulvs(c2Add(B.min, B.max), 0.5f);
1285 float d = c2Abs(c2Dot(n, c2Sub(p0, center_of_b_box))) - c2Dot(abs_n, half_extents);
1286 if (d > 0) return 0;
1288 // Calculate intermediate values up-front.
1289 // This should play well with superscalar architecture.
1290 float da0 = c2SignedDistPointToPlane_OneDimensional(p0.x, -1.0f, B.min.x);
1291 float db0 = c2SignedDistPointToPlane_OneDimensional(p1.x, -1.0f, B.min.x);
1292 float da1 = c2SignedDistPointToPlane_OneDimensional(p0.x, 1.0f, B.max.x);
1293 float db1 = c2SignedDistPointToPlane_OneDimensional(p1.x, 1.0f, B.max.x);
1294 float da2 = c2SignedDistPointToPlane_OneDimensional(p0.y, -1.0f, B.min.y);
1295 float db2 = c2SignedDistPointToPlane_OneDimensional(p1.y, -1.0f, B.min.y);
1296 float da3 = c2SignedDistPointToPlane_OneDimensional(p0.y, 1.0f, B.max.y);
1297 float db3 = c2SignedDistPointToPlane_OneDimensional(p1.y, 1.0f, B.max.y);
1298 float t0 = c2RayToPlane_OneDimensional(da0, db0);
1299 float t1 = c2RayToPlane_OneDimensional(da1, db1);
1300 float t2 = c2RayToPlane_OneDimensional(da2, db2);
1301 float t3 = c2RayToPlane_OneDimensional(da3, db3);
1303 // Calculate hit predicate, no branching.
1304 int hit0 = t0 < 1.0f;
1305 int hit1 = t1 < 1.0f;
1306 int hit2 = t2 < 1.0f;
1307 int hit3 = t3 < 1.0f;
1308 int hit = hit0 | hit1 | hit2 | hit3;
1312 // Remap t's within 0-1 range, where >= 1 is treated as 0.
1313 t0 = (float)hit0 * t0;
1314 t1 = (float)hit1 * t1;
1315 t2 = (float)hit2 * t2;
1316 t3 = (float)hit3 * t3;
1318 // Sort output by finding largest t to deduce the normal.
1319 if (t0 >= t1 && t0 >= t2 && t0 >= t3)
1322 out->n = c2V(-1, 0);
1325 else if (t1 >= t0 && t1 >= t2 && t1 >= t3)
1331 else if (t2 >= t0 && t2 >= t1 && t2 >= t3)
1334 out->n = c2V(0, -1);
1344 } else return 0; // This can still numerically happen.
1347 int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out)
1350 M.y = c2Norm(c2Sub(B.b, B.a));
1353 // rotate capsule to origin, along Y axis
1354 // rotate the ray same way
1355 c2v yBb = c2MulmvT(M, c2Sub(B.b, B.a));
1356 c2v yAp = c2MulmvT(M, c2Sub(A.p, B.a));
1357 c2v yAd = c2MulmvT(M, A.d);
1358 c2v yAe = c2Add(yAp, c2Mulvs(yAd, A.t));
1360 if (yAe.x * yAp.x < 0 || c2Min(c2Abs(yAe.x), c2Abs(yAp.x)) < B.r)
1362 float c = yAp.x > 0 ? B.r : -B.r;
1363 float d = (yAe.x - yAp.x);
1364 float t = (c - yAp.x) / d;
1365 float y = yAp.y + (yAe.y - yAp.y) * t;
1367 // hit bottom half-circle
1373 return c2RaytoCircle(A, C, out);
1376 // hit top-half circle
1382 return c2RaytoCircle(A, C, out);
1385 // hit the middle of capsule
1388 out->n = c > 0 ? M.x : c2Skew(M.y);
1397 int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out)
1399 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
1400 c2v p = c2MulxvT(bx, A.p);
1401 c2v d = c2MulrvT(bx.r, A.d);
1406 // test ray to each plane, tracking lo/hi times of intersection
1407 for (int i = 0; i < B->count; ++i)
1409 float num = c2Dot(B->norms[i], c2Sub(B->verts[i], p));
1410 float den = c2Dot(B->norms[i], d);
1411 if (den == 0 && num < 0) return 0;
1414 if (den < 0 && num < lo * den)
1419 else if (den > 0 && num < hi * den) hi = num / den;
1421 if (hi < lo) return 0;
1427 out->n = c2Mulrv(bx.r, B->norms[index]);
1434 void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m)
1437 c2v d = c2Sub(B.p, A.p);
1438 float d2 = c2Dot(d, d);
1439 float r = A.r + B.r;
1442 float l = c2Sqrt(d2);
1443 c2v n = l != 0 ? c2Mulvs(d, 1.0f / l) : c2V(0, 1.0f);
1445 m->depths[0] = r - l;
1446 m->contact_points[0] = c2Sub(B.p, c2Mulvs(n, B.r));
1451 void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m)
1454 c2v L = c2Clampv(A.p, B.min, B.max);
1455 c2v ab = c2Sub(L, A.p);
1456 float d2 = c2Dot(ab, ab);
1457 float r2 = A.r * A.r;
1460 // shallow (center of circle not inside of AABB)
1463 float d = c2Sqrt(d2);
1466 m->depths[0] = A.r - d;
1467 m->contact_points[0] = c2Add(A.p, c2Mulvs(n, d));
1471 // deep (center of circle inside of AABB)
1472 // clamp circle's center to edge of AABB, then form the manifold
1475 c2v mid = c2Mulvs(c2Add(B.min, B.max), 0.5f);
1476 c2v e = c2Mulvs(c2Sub(B.max, B.min), 0.5f);
1477 c2v d = c2Sub(A.p, mid);
1478 c2v abs_d = c2Absv(d);
1480 float x_overlap = e.x - abs_d.x;
1481 float y_overlap = e.y - abs_d.y;
1486 if (x_overlap < y_overlap)
1490 n = c2Mulvs(n, d.x < 0 ? 1.0f : -1.0f);
1497 n = c2Mulvs(n, d.y < 0 ? 1.0f : -1.0f);
1501 m->depths[0] = A.r + depth;
1502 m->contact_points[0] = c2Sub(A.p, c2Mulvs(n, depth));
1508 void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m)
1512 float r = A.r + B.r;
1513 float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0);
1517 if (d == 0) n = c2Norm(c2Skew(c2Sub(B.b, B.a)));
1518 else n = c2Norm(c2Sub(b, a));
1521 m->depths[0] = r - d;
1522 m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
1527 void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m)
1530 c2v mid_a = c2Mulvs(c2Add(A.min, A.max), 0.5f);
1531 c2v mid_b = c2Mulvs(c2Add(B.min, B.max), 0.5f);
1532 c2v eA = c2Absv(c2Mulvs(c2Sub(A.max, A.min), 0.5f));
1533 c2v eB = c2Absv(c2Mulvs(c2Sub(B.max, B.min), 0.5f));
1534 c2v d = c2Sub(mid_b, mid_a);
1536 // calc overlap on x and y axes
1537 float dx = eA.x + eB.x - c2Abs(d.x);
1539 float dy = eA.y + eB.y - c2Abs(d.y);
1546 // x axis overlap is smaller
1553 p = c2Sub(mid_a, c2V(eA.x, 0));
1558 p = c2Add(mid_a, c2V(eA.x, 0));
1562 // y axis overlap is smaller
1569 p = c2Sub(mid_a, c2V(0, eA.y));
1574 p = c2Add(mid_a, c2V(0, eA.y));
1579 m->contact_points[0] = p;
1580 m->depths[0] = depth;
1584 void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m)
1588 c2BBVerts(p.verts, &A);
1590 c2Norms(p.verts, p.norms, 4);
1591 c2CapsuletoPolyManifold(B, &p, 0, m);
1595 void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m)
1599 float r = A.r + B.r;
1600 float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0);
1604 if (d == 0) n = c2Norm(c2Skew(c2Sub(A.b, A.a)));
1605 else n = c2Norm(c2Sub(b, a));
1608 m->depths[0] = r - d;
1609 m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
1614 static C2_INLINE c2h c2PlaneAt(const c2Poly* p, const int i)
1618 h.d = c2Dot(p->norms[i], p->verts[i]);
1622 void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx_tr, c2Manifold* m)
1626 float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx_tr, &a, &b, 0, 0, 0);
1628 // shallow, the circle center did not hit the polygon
1629 // just use a and b from GJK to define the collision
1632 c2v n = c2Sub(b, a);
1633 float l = c2Dot(n, n);
1638 m->contact_points[0] = b;
1639 m->depths[0] = A.r - l;
1640 m->n = c2Mulvs(n, 1.0f / l);
1644 // Circle center is inside the polygon
1645 // find the face closest to circle center to form manifold
1648 c2x bx = bx_tr ? *bx_tr : c2xIdentity();
1649 float sep = -FLT_MAX;
1651 c2v local = c2MulxvT(bx, A.p);
1653 for (int i = 0; i < B->count; ++i)
1655 c2h h = c2PlaneAt(B, i);
1656 d = c2Dist(h, local);
1657 if (d > A.r) return;
1665 c2h h = c2PlaneAt(B, index);
1666 c2v p = c2Project(h, local);
1668 m->contact_points[0] = c2Mulxv(bx, p);
1669 m->depths[0] = A.r - sep;
1670 m->n = c2Neg(c2Mulrv(bx.r, B->norms[index]));
1674 // Forms a c2Poly and uses c2PolytoPolyManifold
1675 void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m)
1679 c2BBVerts(p.verts, &A);
1681 c2Norms(p.verts, p.norms, 4);
1682 c2PolytoPolyManifold(&p, 0, B, bx, m);
1685 // clip a segment to a plane
1686 static int c2Clip(c2v* seg, c2h h)
1691 if ((d0 = c2Dist(h, seg[0])) < 0) out[sp++] = seg[0];
1692 if ((d1 = c2Dist(h, seg[1])) < 0) out[sp++] = seg[1];
1693 if (d0 == 0 && d1 == 0) {
1696 } else if (d0 * d1 <= 0) out[sp++] = c2Intersect(seg[0], seg[1], d0, d1);
1697 seg[0] = out[0]; seg[1] = out[1];
1702 #pragma warning(push)
1703 #pragma warning(disable:4204) // nonstandard extension used: non-constant aggregate initializer
1706 // clip a segment to the "side planes" of another segment.
1707 // side planes are planes orthogonal to a segment and attached to the
1708 // endpoints of the segment
1709 static int c2SidePlanes(c2v* seg, c2x x, const c2Poly* p, int e, c2h* h)
1711 c2v ra = c2Mulxv(x, p->verts[e]);
1712 c2v rb = c2Mulxv(x, p->verts[e + 1 == p->count ? 0 : e + 1]);
1713 c2v in = c2Norm(c2Sub(rb, ra));
1714 c2h left = { c2Neg(in), c2Dot(c2Neg(in), ra) };
1715 c2h right = { in, c2Dot(in, rb) };
1716 if (c2Clip(seg, left) < 2) return 0;
1717 if (c2Clip(seg, right) < 2) return 0;
1720 h->d = c2Dot(c2CCW90(in), ra);
1725 static void c2KeepDeep(c2v* seg, c2h h, c2Manifold* m)
1728 for (int i = 0; i < 2; ++i)
1731 float d = c2Dist(h, p);
1734 m->contact_points[cp] = p;
1743 static C2_INLINE c2v c2CapsuleSupport(c2Capsule A, c2v dir)
1745 float da = c2Dot(A.a, dir);
1746 float db = c2Dot(A.b, dir);
1747 if (da > db) return c2Add(A.a, c2Mulvs(dir, A.r));
1748 else return c2Add(A.b, c2Mulvs(dir, A.r));
1751 static void c2AntinormalFace(c2Capsule cap, const c2Poly* p, c2x x, int* face_out, c2v* n_out)
1753 float sep = -FLT_MAX;
1756 for (int i = 0; i < p->count; ++i)
1758 c2h h = c2Mulxh(x, c2PlaneAt(p, i));
1759 c2v n0 = c2Neg(h.n);
1760 c2v s = c2CapsuleSupport(cap, n0);
1761 float d = c2Dist(h, s);
1773 void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m)
1777 float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx_ptr, &a, &b, 0, 0, 0);
1779 // deep, treat as segment to poly collision
1782 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
1785 c2AntinormalFace(A, B, bx, &index, &n);
1786 c2v seg[2] = { A.a, A.b };
1788 if (!c2SidePlanes(seg, bx, B, index, &h)) return;
1789 c2KeepDeep(seg, h, m);
1790 for (int i = 0; i < m->count; ++i)
1792 m->depths[i] += c2Sign(m->depths) * A.r;
1793 m->contact_points[i] = c2Add(m->contact_points[i], c2Mulvs(n, A.r));
1798 // shallow, use GJK results a and b to define manifold
1801 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
1802 c2v ab = c2Sub(b, a);
1805 for (int i = 0; i < B->count; ++i)
1807 c2v n = c2Mulrv(bx.r, B->norms[i]);
1808 if (c2Parallel(c2Neg(ab), n, 5.0e-3f))
1821 m->contact_points[0] = c2Add(a, c2Mulvs(m->n, A.r));
1822 m->depths[0] = A.r - d;
1825 // 2 contacts if laying on a polygon face nicely
1830 c2AntinormalFace(A, B, bx, &index, &n);
1831 c2v seg[2] = { c2Add(A.a, c2Mulvs(n, A.r)), c2Add(A.b, c2Mulvs(n, A.r)) };
1833 if (!c2SidePlanes(seg, bx, B, index, &h)) goto one_contact;
1834 c2KeepDeep(seg, h, m);
1841 #pragma warning(pop)
1844 static float c2CheckFaces(const c2Poly* A, c2x ax, const c2Poly* B, c2x bx, int* face_index)
1846 c2x b_in_a = c2MulxxT(ax, bx);
1847 c2x a_in_b = c2MulxxT(bx, ax);
1848 float sep = -FLT_MAX;
1851 for (int i = 0; i < A->count; ++i)
1853 c2h h = c2PlaneAt(A, i);
1854 int idx = c2Support(B->verts, B->count, c2Mulrv(a_in_b.r, c2Neg(h.n)));
1855 c2v p = c2Mulxv(b_in_a, B->verts[idx]);
1856 float d = c2Dist(h, p);
1864 *face_index = index;
1868 static C2_INLINE void c2Incident(c2v* incident, const c2Poly* ip, c2x ix, const c2Poly* rp, c2x rx, int re)
1870 c2v n = c2MulrvT(ix.r, c2Mulrv(rx.r, rp->norms[re]));
1872 float min_dot = FLT_MAX;
1873 for (int i = 0; i < ip->count; ++i)
1875 float dot = c2Dot(n, ip->norms[i]);
1882 incident[0] = c2Mulxv(ix, ip->verts[index]);
1883 incident[1] = c2Mulxv(ix, ip->verts[index + 1 == ip->count ? 0 : index + 1]);
1886 // Please see Dirk Gregorius's 2013 GDC lecture on the Separating Axis Theorem
1887 // for a full-algorithm overview. The short description is:
1888 // Test A against faces of B, test B against faces of A
1889 // Define the reference and incident shapes (denoted by r and i respectively)
1890 // Define the reference face as the axis of minimum penetration
1891 // Find the incident face, which is most anti-normal face
1892 // Clip incident face to reference face side planes
1893 // Keep all points behind the reference face
1894 void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax_ptr, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m)
1897 c2x ax = ax_ptr ? *ax_ptr : c2xIdentity();
1898 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
1901 if ((sa = c2CheckFaces(A, ax, B, bx, &ea)) >= 0) return;
1902 if ((sb = c2CheckFaces(B, bx, A, ax, &eb)) >= 0) return;
1904 const c2Poly* rp,* ip;
1907 float kRelTol = 0.95f, kAbsTol = 0.01f;
1909 if (sa * kRelTol > sb + kAbsTol)
1925 c2Incident(incident, ip, ix, rp, rx, re);
1927 if (!c2SidePlanes(incident, rx, rp, re, &rh)) return;
1928 c2KeepDeep(incident, rh, m);
1929 if (flip) m->n = c2Neg(m->n);
1932 #endif // CUTE_C2_IMPLEMENTATION_ONCE
1933 #endif // CUTE_C2_IMPLEMENTATION
1936 ------------------------------------------------------------------------------
1937 This software is available under 2 licenses - you may choose the one you like.
1938 ------------------------------------------------------------------------------
1939 ALTERNATIVE A - zlib license
1940 Copyright (c) 2017 Randy Gaul http://www.randygaul.net
1941 This software is provided 'as-is', without any express or implied warranty.
1942 In no event will the authors be held liable for any damages arising from
1943 the use of this software.
1944 Permission is granted to anyone to use this software for any purpose,
1945 including commercial applications, and to alter it and redistribute it
1946 freely, subject to the following restrictions:
1947 1. The origin of this software must not be misrepresented; you must not
1948 claim that you wrote the original software. If you use this software
1949 in a product, an acknowledgment in the product documentation would be
1950 appreciated but is not required.
1951 2. Altered source versions must be plainly marked as such, and must not
1952 be misrepresented as being the original software.
1953 3. This notice may not be removed or altered from any source distribution.
1954 ------------------------------------------------------------------------------
1955 ALTERNATIVE B - Public Domain (www.unlicense.org)
1956 This is free and unencumbered software released into the public domain.
1957 Anyone is free to copy, modify, publish, use, compile, sell, or distribute this
1958 software, either in source code form or as a compiled binary, for any purpose,
1959 commercial or non-commercial, and by any means.
1960 In jurisdictions that recognize copyright laws, the author or authors of this
1961 software dedicate any and all copyright interest in the software to the public
1962 domain. We make this dedication for the benefit of the public at large and to
1963 the detriment of our heirs and successors. We intend this dedication to be an
1964 overt act of relinquishment in perpetuity of all present and future rights to
1965 this software under copyright law.
1966 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
1967 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
1968 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
1969 AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
1970 ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
1971 WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
1972 ------------------------------------------------------------------------------