1 // Copyright (c) 2017 Randy Gaul http://www.randygaul.net
2 // SPDX-FileCopyrightText: 2017 Randy Gaul http://www.randygaul.net
3 // SPDX-FileCopyrightText: 2021 Randy Gaul http://www.randygaul.net
5 // SPDX-License-Identifier: Unlicense
6 // SPDX-License-Identifier: Zlib
8 ------------------------------------------------------------------------------
9 Licensing information can be found at the end of the file.
10 ------------------------------------------------------------------------------
14 To create implementation (the function definitions)
15 #define CUTE_C2_IMPLEMENTATION
16 in *one* C/CPP file (translation unit) that includes this file
21 cute_c2 is a single-file header that implements 2D collision detection routines
22 that test for overlap, and optionally can find the collision manifold. The
23 manifold contains all necessary information to prevent shapes from inter-
24 penetrating, which is useful for character controllers, general physics
25 simulation, and user-interface programming.
27 This header implements a group of "immediate mode" functions that should be
28 very easily adapted into pre-existing projects.
33 Most of the math types in this header are for internal use. Users care about
34 the shape types and the collision functions.
43 COLLISION FUNCTIONS (*** is a shape name from the above list):
44 * c2***to*** - boolean YES/NO hittest
45 * c2***to***Manifold - construct manifold to describe how shapes hit
46 * c2GJK - runs GJK algorithm to find closest point pair
48 * c2TOI - computes the time of impact between two shapes, useful for
49 sweeping shapes, or doing shape casts
50 * c2MakePoly - Runs convex hull algorithm and computes normals on input point-set
51 * c2Collided - generic version of c2***to*** funcs
52 * c2Collide - generic version of c2***to***Manifold funcs
53 * c2CastRay - generic version of c2Rayto*** funcs
55 The rest of the header is more or less for internal use. Here is an example of
56 making some shapes and testing for collision:
63 cap.a = first_endpoint;
64 cap.b = second_endpoint;
67 int hit = c2CircletoCapsule(c, cap);
70 handle collision here...
73 For more code examples and tests please see:
74 https://github.com/RandyGaul/cute_header/tree/master/examples_cute_gl_and_c2
76 Here is a past discussion thread on this header:
77 https://www.reddit.com/r/gamedev/comments/5tqyey/tinyc2_2d_collision_detection_library_in_c/
79 Here is a very nice repo containing various tests and examples using SFML for rendering:
80 https://github.com/sro5h/tinyc2-tests
85 This header does not implement a broad-phase, and instead concerns itself with
86 the narrow-phase. This means this header just checks to see if two individual
87 shapes are touching, and can give information about how they are touching.
89 Very common 2D broad-phases are tree and grid approaches. Quad trees are good
90 for static geometry that does not move much if at all. Dynamic AABB trees are
91 good for general purpose use, and can handle moving objects very well. Grids
92 are great and are similar to quad trees.
94 If implementing a grid it can be wise to have each collideable grid cell hold
95 an integer. This integer refers to a 2D shape that can be passed into the
96 various functions in this header. The shape can be transformed from "model"
97 space to "world" space using c2x -- a transform struct. In this way a grid
98 can be implemented that holds any kind of convex shape (that this header
99 supports) while conserving memory with shape instancing.
101 Please email at my address with any questions or comments at:
102 author's last name followed by 1748 at gmail
107 * Circles, capsules, AABBs, rays and convex polygons are supported
108 * Fast boolean only result functions (hit yes/no)
109 * Slghtly slower manifold generation for collision normals + depths +points
110 * GJK implementation (finds closest points for disjoint pairs of shapes)
111 * Shape casts/sweeps with c2TOI function (time of impact)
112 * Robust 2D convex hull generator
113 * Lots of correctly implemented and tested 2D math routines
114 * Implemented in portable C, and is readily portable to other languages
115 * Generic c2Collide, c2Collided and c2CastRay function (can pass in any shape type)
116 * Extensive examples at: https://github.com/RandyGaul/cute_headers/tree/master/examples_cute_gl_and_c2
121 1.0 (02/13/2017) initial release
122 1.01 (02/13/2017) const crusade, minor optimizations, capsule degen
123 1.02 (03/21/2017) compile fixes for c on more compilers
124 1.03 (09/15/2017) various bugfixes and quality of life changes to manifolds
125 1.04 (03/25/2018) fixed manifold bug in c2CircletoAABBManifold
126 1.05 (11/01/2018) added c2TOI (time of impact) for shape cast/sweep test
127 1.06 (08/23/2019) C2_*** types to C2_TYPE_***, and CUTE_C2_API
132 Plastburk 1.01 - const pointers pull request
133 mmozeiko 1.02 - 3 compile bugfixes
134 felipefs 1.02 - 3 compile bugfixes
135 seemk 1.02 - fix branching bug in c2Collide
136 sro5h 1.02 - bug reports for multiple manifold funcs
137 sro5h 1.03 - work involving quality of life fixes for manifolds
138 Wizzard033 1.06 - C2_*** types to C2_TYPE_***, and CUTE_C2_API
141 #if !defined(CUTE_C2_H)
143 // this can be adjusted as necessary, but is highly recommended to be kept at 8.
144 // higher numbers will incur quite a bit of memory overhead, and convex shapes
145 // over 8 verts start to just look like spheres, which can be implicitly rep-
146 // resented as a point + radius. usually tools that generate polygons should be
147 // constructed so they do not output polygons with too many verts.
148 // Note: polygons in cute_c2 are all *convex*.
149 #define C2_MAX_POLYGON_VERTS 12
158 // 2d rotation composed of cos/sin pair
165 // 2d rotation matrix
172 // 2d transformation "x"
173 // These are used especially for c2Poly when a c2Poly is passed to a function.
174 // Since polygons are prime for "instancing" a c2x transform can be used to
175 // transform a polygon from local space to world space. In functions that take
176 // a c2x pointer (like c2PolytoPoly), these pointers can be NULL, which represents
177 // an identity transformation and assumes the verts inside of c2Poly are already
185 // 2d halfspace (aka plane, aka line)
188 c2v n; // normal, normalized
189 float d; // distance to origin from plane, or ax + by = d
204 // a capsule is defined as a line segment (from a to b) and radius r
215 c2v verts[C2_MAX_POLYGON_VERTS];
216 c2v norms[C2_MAX_POLYGON_VERTS];
220 // Many algorithms in this file are sensitive to the magnitude of the
221 // ray direction (c2Ray::d). It is highly recommended to normalize the
222 // ray direction and use t to specify a distance. Please see this link
223 // for an in-depth explanation: https://github.com/RandyGaul/cute_headers/issues/30
227 c2v d; // direction (normalized)
228 float t; // distance along d from position p to find endpoint of ray
233 float t; // time of impact
234 c2v n; // normal of surface at impact (unit length)
237 // position of impact p = ray.p + ray.d * raycast.t
238 #define c2Impact(ray, t) c2Add(ray.p, c2Mulvs(ray.d, t))
240 // contains all information necessary to resolve a collision, or in other words
241 // this is the information needed to separate shapes that are colliding. Doing
242 // the resolution step is *not* included in cute_c2. cute_c2 does not include
243 // "feature information" that describes which topological features collided.
244 // However, modifying the exist ***Manifold funcs can be done to output any
245 // needed feature information. Feature info is sometimes needed for certain kinds
246 // of simulations that cache information over multiple game-ticks, of which are
247 // associated to the collision of specific features. An example implementation
248 // is in the qu3e 3D physics engine library: https://github.com/RandyGaul/qu3e
253 c2v contact_points[2];
255 // always points from shape A to shape B (first and second shapes passed into
256 // any of the c2***to***Manifold functions)
260 // This define allows exporting/importing of the header to a dynamic library.
261 // Here's an example.
262 // #define CUTE_C2_API extern "C" __declspec(dllexport)
263 #if !defined(CUTE_C2_API)
267 // boolean collision detection
268 // these versions are faster than the manifold versions, but only give a YES/NO
270 CUTE_C2_API int c2CircletoCircle(c2Circle A, c2Circle B);
271 CUTE_C2_API int c2CircletoAABB(c2Circle A, c2AABB B);
272 CUTE_C2_API int c2CircletoCapsule(c2Circle A, c2Capsule B);
273 CUTE_C2_API int c2AABBtoAABB(c2AABB A, c2AABB B);
274 CUTE_C2_API int c2AABBtoCapsule(c2AABB A, c2Capsule B);
275 CUTE_C2_API int c2CapsuletoCapsule(c2Capsule A, c2Capsule B);
276 CUTE_C2_API int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx);
277 CUTE_C2_API int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx);
278 CUTE_C2_API int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx);
279 CUTE_C2_API int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx);
282 // output is placed into the c2Raycast struct, which represents the hit location
283 // of the ray. the out param contains no meaningful information if these funcs
285 CUTE_C2_API int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out);
286 CUTE_C2_API int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out);
287 CUTE_C2_API int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out);
288 CUTE_C2_API int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out);
290 // manifold generation
291 // these functions are slower than the boolean versions, but will compute one
292 // or two points that represent the plane of contact. This information is
293 // is usually needed to resolve and prevent shapes from colliding. If no coll
294 // ision occured the count member of the manifold struct is set to 0.
295 CUTE_C2_API void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m);
296 CUTE_C2_API void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m);
297 CUTE_C2_API void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m);
298 CUTE_C2_API void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m);
299 CUTE_C2_API void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m);
300 CUTE_C2_API void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m);
301 CUTE_C2_API void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx, c2Manifold* m);
302 CUTE_C2_API void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m);
303 CUTE_C2_API void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx, c2Manifold* m);
304 CUTE_C2_API void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx, c2Manifold* m);
315 // This struct is only for advanced usage of the c2GJK function. See comments inside of the
316 // c2GJK function for more details.
326 // Runs the GJK algorithm to find closest points, returns distance between closest points.
327 // outA and outB can be NULL, in this case only distance is returned. ax_ptr and bx_ptr
328 // can be NULL, and represent local to world transformations for shapes A and B respectively.
329 // use_radius will apply radii for capsules and circles (if set to false, spheres are
330 // treated as points and capsules are treated as line segments i.e. rays). The cache parameter
331 // should be NULL, as it is only for advanced usage (unless you know what you're doing, then
332 // go ahead and use it). iterations is an optional parameter.
333 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);
335 // Computes the time of impact from shape A and shape B. The velocity of each shape is provided
336 // by vA and vB respectively. The shapes are *not* allowed to rotate over time. The velocity is
337 // assumed to represent the change in motion from time 0 to time 1, and so the return value will
338 // be a number from 0 to 1. To move each shape to the colliding configuration, multiply vA and vB
339 // each by the return value. ax_ptr and bx_ptr are optional parameters to transforms for each shape,
340 // and are typically used for polygon shapes to transform from model to world space. Set these to
341 // NULL to represent identity transforms. The out_normal for non-colliding configurations (or in
342 // other words, when the return value is 1) is just the direction pointing along the closest points
343 // from shape A to shape B. out_normal can be NULL. iterations is an optional parameter. use_radius
344 // will apply radii for capsules and circles (if set to false, spheres are treated as points and
345 // capsules are treated as line segments i.e. rays).
346 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);
348 // Computes 2D convex hull. Will not do anything if less than two verts supplied. If
349 // more than C2_MAX_POLYGON_VERTS are supplied extras are ignored.
350 CUTE_C2_API int c2Hull(c2v* verts, int count);
351 CUTE_C2_API void c2Norms(c2v* verts, c2v* norms, int count);
353 // runs c2Hull and c2Norms, assumes p->verts and p->count are both set to valid values
354 CUTE_C2_API void c2MakePoly(c2Poly* p);
356 // Generic collision detection routines, useful for games that want to use some poly-
357 // morphism to write more generic-styled code. Internally calls various above functions.
358 // For AABBs/Circles/Capsules ax and bx are ignored. For polys ax and bx can define
359 // model to world transformations, or be NULL for identity transforms.
360 CUTE_C2_API int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB);
361 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);
362 CUTE_C2_API int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out);
365 #define C2_INLINE __forceinline
367 #define C2_INLINE inline __attribute__((always_inline))
370 // adjust these primitives as seen fit
371 #include <string.h> // memcpy
373 #define c2Sin(radians) sinf(radians)
374 #define c2Cos(radians) cosf(radians)
375 #define c2Sqrt(a) sqrtf(a)
376 #define c2Min(a, b) ((a) < (b) ? (a) : (b))
377 #define c2Max(a, b) ((a) > (b) ? (a) : (b))
378 #define c2Abs(a) ((a) < 0 ? -(a) : (a))
379 #define c2Clamp(a, lo, hi) c2Max(lo, c2Min(a, hi))
380 C2_INLINE void c2SinCos(float radians, float* s, float* c) { *c = c2Cos(radians); *s = c2Sin(radians); }
381 #define c2Sign(a) (a < 0 ? -1.0f : 1.0f)
383 // The rest of the functions in the header-only portion are all for internal use
384 // and use the author's personal naming conventions. It is recommended to use one's
385 // own math library instead of the one embedded here in cute_c2, but for those
386 // curious or interested in trying it out here's the details:
388 // The Mul functions are used to perform multiplication. x stands for transform,
389 // v stands for vector, s stands for scalar, r stands for rotation, h stands for
390 // halfspace and T stands for transpose.For example c2MulxvT stands for "multiply
391 // a transform with a vector, and transpose the transform".
394 C2_INLINE c2v c2V(float x, float y) { c2v a; a.x = x; a.y = y; return a; }
395 C2_INLINE c2v c2Add(c2v a, c2v b) { a.x += b.x; a.y += b.y; return a; }
396 C2_INLINE c2v c2Sub(c2v a, c2v b) { a.x -= b.x; a.y -= b.y; return a; }
397 C2_INLINE float c2Dot(c2v a, c2v b) { return a.x * b.x + a.y * b.y; }
398 C2_INLINE c2v c2Mulvs(c2v a, float b) { a.x *= b; a.y *= b; return a; }
399 C2_INLINE c2v c2Mulvv(c2v a, c2v b) { a.x *= b.x; a.y *= b.y; return a; }
400 C2_INLINE c2v c2Div(c2v a, float b) { return c2Mulvs(a, 1.0f / b); }
401 C2_INLINE c2v c2Skew(c2v a) { c2v b; b.x = -a.y; b.y = a.x; return b; }
402 C2_INLINE c2v c2CCW90(c2v a) { c2v b; b.x = a.y; b.y = -a.x; return b; }
403 C2_INLINE float c2Det2(c2v a, c2v b) { return a.x * b.y - a.y * b.x; }
404 C2_INLINE c2v c2Minv(c2v a, c2v b) { return c2V(c2Min(a.x, b.x), c2Min(a.y, b.y)); }
405 C2_INLINE c2v c2Maxv(c2v a, c2v b) { return c2V(c2Max(a.x, b.x), c2Max(a.y, b.y)); }
406 C2_INLINE c2v c2Clampv(c2v a, c2v lo, c2v hi) { return c2Maxv(lo, c2Minv(a, hi)); }
407 C2_INLINE c2v c2Absv(c2v a) { return c2V(c2Abs(a.x), c2Abs(a.y)); }
408 C2_INLINE float c2Hmin(c2v a) { return c2Min(a.x, a.y); }
409 C2_INLINE float c2Hmax(c2v a) { return c2Max(a.x, a.y); }
410 C2_INLINE float c2Len(c2v a) { return c2Sqrt(c2Dot(a, a)); }
411 C2_INLINE c2v c2Norm(c2v a) { return c2Div(a, c2Len(a)); }
412 C2_INLINE c2v c2SafeNorm(c2v a) { float sq = c2Dot(a, a); return sq ? c2Div(a, c2Len(a)) : c2V(0, 0); }
413 C2_INLINE c2v c2Neg(c2v a) { return c2V(-a.x, -a.y); }
414 C2_INLINE c2v c2Lerp(c2v a, c2v b, float t) { return c2Add(a, c2Mulvs(c2Sub(b, a), t)); }
415 C2_INLINE int c2Parallel(c2v a, c2v b, float kTol)
417 float k = c2Len(a) / c2Len(b);
419 if (c2Abs(a.x - b.x) < kTol && c2Abs(a.y - b.y) < kTol) return 1;
424 C2_INLINE c2r c2Rot(float radians) { c2r r; c2SinCos(radians, &r.s, &r.c); return r; }
425 C2_INLINE c2r c2RotIdentity() { c2r r; r.c = 1.0f; r.s = 0; return r; }
426 C2_INLINE c2v c2RotX(c2r r) { return c2V(r.c, r.s); }
427 C2_INLINE c2v c2RotY(c2r r) { return c2V(-r.s, r.c); }
428 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); }
429 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); }
430 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; }
431 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; }
433 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; }
434 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; }
435 C2_INLINE c2m c2Mulmm(c2m a, c2m b) { c2m c; c.x = c2Mulmv(a, b.x); c.y = c2Mulmv(a, b.y); return c; }
436 C2_INLINE c2m c2MulmmT(c2m a, c2m b) { c2m c; c.x = c2MulmvT(a, b.x); c.y = c2MulmvT(a, b.y); return c; }
439 C2_INLINE c2x c2xIdentity() { c2x x; x.p = c2V(0, 0); x.r = c2RotIdentity(); return x; }
440 C2_INLINE c2v c2Mulxv(c2x a, c2v b) { return c2Add(c2Mulrv(a.r, b), a.p); }
441 C2_INLINE c2v c2MulxvT(c2x a, c2v b) { return c2MulrvT(a.r, c2Sub(b, a.p)); }
442 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; }
443 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; }
444 C2_INLINE c2x c2Transform(c2v p, float radians) { c2x x; x.r = c2Rot(radians); x.p = p; return x; }
447 C2_INLINE c2v c2Origin(c2h h) { return c2Mulvs(h.n, h.d); }
448 C2_INLINE float c2Dist(c2h h, c2v p) { return c2Dot(h.n, p) - h.d; }
449 C2_INLINE c2v c2Project(c2h h, c2v p) { return c2Sub(p, c2Mulvs(h.n, c2Dist(h, p))); }
450 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; }
451 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; }
452 C2_INLINE c2v c2Intersect(c2v a, c2v b, float da, float db) { return c2Add(a, c2Mulvs(c2Sub(b, a), (da / (da - db)))); }
454 C2_INLINE void c2BBVerts(c2v* out, c2AABB* bb)
457 out[1] = c2V(bb->max.x, bb->min.y);
459 out[3] = c2V(bb->min.x, bb->max.y);
465 #ifdef CUTE_C2_IMPLEMENTATION
466 #ifndef CUTE_C2_IMPLEMENTATION_ONCE
467 #define CUTE_C2_IMPLEMENTATION_ONCE
469 int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB)
476 case C2_TYPE_CIRCLE: return c2CircletoCircle(*(c2Circle*)A, *(c2Circle*)B);
477 case C2_TYPE_AABB: return c2CircletoAABB(*(c2Circle*)A, *(c2AABB*)B);
478 case C2_TYPE_CAPSULE: return c2CircletoCapsule(*(c2Circle*)A, *(c2Capsule*)B);
479 case C2_TYPE_POLY: return c2CircletoPoly(*(c2Circle*)A, (const c2Poly*)B, bx);
487 case C2_TYPE_CIRCLE: return c2CircletoAABB(*(c2Circle*)B, *(c2AABB*)A);
488 case C2_TYPE_AABB: return c2AABBtoAABB(*(c2AABB*)A, *(c2AABB*)B);
489 case C2_TYPE_CAPSULE: return c2AABBtoCapsule(*(c2AABB*)A, *(c2Capsule*)B);
490 case C2_TYPE_POLY: return c2AABBtoPoly(*(c2AABB*)A, (const c2Poly*)B, bx);
495 case C2_TYPE_CAPSULE:
498 case C2_TYPE_CIRCLE: return c2CircletoCapsule(*(c2Circle*)B, *(c2Capsule*)A);
499 case C2_TYPE_AABB: return c2AABBtoCapsule(*(c2AABB*)B, *(c2Capsule*)A);
500 case C2_TYPE_CAPSULE: return c2CapsuletoCapsule(*(c2Capsule*)A, *(c2Capsule*)B);
501 case C2_TYPE_POLY: return c2CapsuletoPoly(*(c2Capsule*)A, (const c2Poly*)B, bx);
509 case C2_TYPE_CIRCLE: return c2CircletoPoly(*(c2Circle*)B, (const c2Poly*)A, ax);
510 case C2_TYPE_AABB: return c2AABBtoPoly(*(c2AABB*)B, (const c2Poly*)A, ax);
511 case C2_TYPE_CAPSULE: return c2CapsuletoPoly(*(c2Capsule*)B, (const c2Poly*)A, ax);
512 case C2_TYPE_POLY: return c2PolytoPoly((const c2Poly*)A, ax, (const c2Poly*)B, bx);
522 void c2Collide(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB, c2Manifold* m)
531 case C2_TYPE_CIRCLE: c2CircletoCircleManifold(*(c2Circle*)A, *(c2Circle*)B, m); break;
532 case C2_TYPE_AABB: c2CircletoAABBManifold(*(c2Circle*)A, *(c2AABB*)B, m); break;
533 case C2_TYPE_CAPSULE: c2CircletoCapsuleManifold(*(c2Circle*)A, *(c2Capsule*)B, m); break;
534 case C2_TYPE_POLY: c2CircletoPolyManifold(*(c2Circle*)A, (const c2Poly*)B, bx, m); break;
541 case C2_TYPE_CIRCLE: c2CircletoAABBManifold(*(c2Circle*)B, *(c2AABB*)A, m); m->n = c2Neg(m->n); break;
542 case C2_TYPE_AABB: c2AABBtoAABBManifold(*(c2AABB*)A, *(c2AABB*)B, m); break;
543 case C2_TYPE_CAPSULE: c2AABBtoCapsuleManifold(*(c2AABB*)A, *(c2Capsule*)B, m); break;
544 case C2_TYPE_POLY: c2AABBtoPolyManifold(*(c2AABB*)A, (const c2Poly*)B, bx, m); break;
548 case C2_TYPE_CAPSULE:
551 case C2_TYPE_CIRCLE: c2CircletoCapsuleManifold(*(c2Circle*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
552 case C2_TYPE_AABB: c2AABBtoCapsuleManifold(*(c2AABB*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
553 case C2_TYPE_CAPSULE: c2CapsuletoCapsuleManifold(*(c2Capsule*)A, *(c2Capsule*)B, m); break;
554 case C2_TYPE_POLY: c2CapsuletoPolyManifold(*(c2Capsule*)A, (const c2Poly*)B, bx, m); break;
561 case C2_TYPE_CIRCLE: c2CircletoPolyManifold(*(c2Circle*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
562 case C2_TYPE_AABB: c2AABBtoPolyManifold(*(c2AABB*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
563 case C2_TYPE_CAPSULE: c2CapsuletoPolyManifold(*(c2Capsule*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
564 case C2_TYPE_POLY: c2PolytoPolyManifold((const c2Poly*)A, ax, (const c2Poly*)B, bx, m); break;
570 int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out)
574 case C2_TYPE_CIRCLE: return c2RaytoCircle(A, *(c2Circle*)B, out);
575 case C2_TYPE_AABB: return c2RaytoAABB(A, *(c2AABB*)B, out);
576 case C2_TYPE_CAPSULE: return c2RaytoCapsule(A, *(c2Capsule*)B, out);
577 case C2_TYPE_POLY: return c2RaytoPoly(A, (const c2Poly*)B, bx, out);
583 #define C2_GJK_ITERS 20
589 c2v verts[C2_MAX_POLYGON_VERTS];
609 static C2_INLINE void c2MakeProxy(const void* shape, C2_TYPE type, c2Proxy* p)
615 c2Circle* c = (c2Circle*)shape;
623 c2AABB* bb = (c2AABB*)shape;
626 c2BBVerts(p->verts, bb);
629 case C2_TYPE_CAPSULE:
631 c2Capsule* c = (c2Capsule*)shape;
640 c2Poly* poly = (c2Poly*)shape;
642 p->count = poly->count;
643 for (int i = 0; i < p->count; ++i) p->verts[i] = poly->verts[i];
648 static C2_INLINE int c2Support(const c2v* verts, int count, c2v d)
651 float dmax = c2Dot(verts[0], d);
653 for (int i = 1; i < count; ++i)
655 float dot = c2Dot(verts[i], d);
666 #define C2_BARY(n, x) c2Mulvs(s->n.x, (den * s->n.u))
667 #define C2_BARY2(x) c2Add(C2_BARY(a, x), C2_BARY(b, x))
668 #define C2_BARY3(x) c2Add(c2Add(C2_BARY(a, x), C2_BARY(b, x)), C2_BARY(c, x))
670 static C2_INLINE c2v c2L(c2Simplex* s)
672 float den = 1.0f / s->div;
675 case 1: return s->a.p;
676 case 2: return C2_BARY2(p);
677 case 3: return C2_BARY3(p);
678 default: return c2V(0, 0);
682 static C2_INLINE void c2Witness(c2Simplex* s, c2v* a, c2v* b)
684 float den = 1.0f / s->div;
687 case 1: *a = s->a.sA; *b = s->a.sB; break;
688 case 2: *a = C2_BARY2(sA); *b = C2_BARY2(sB); break;
689 case 3: *a = C2_BARY3(sA); *b = C2_BARY3(sB); break;
690 default: *a = c2V(0, 0); *b = c2V(0, 0);
694 static C2_INLINE c2v c2D(c2Simplex* s)
698 case 1: return c2Neg(s->a.p);
701 c2v ab = c2Sub(s->b.p, s->a.p);
702 if (c2Det2(ab, c2Neg(s->a.p)) > 0) return c2Skew(ab);
706 default: return c2V(0, 0);
710 static C2_INLINE void c22(c2Simplex* s)
714 float u = c2Dot(b, c2Norm(c2Sub(b, a)));
715 float v = c2Dot(a, c2Norm(c2Sub(a, b)));
741 static C2_INLINE void c23(c2Simplex* s)
747 float uAB = c2Dot(b, c2Norm(c2Sub(b, a)));
748 float vAB = c2Dot(a, c2Norm(c2Sub(a, b)));
749 float uBC = c2Dot(c, c2Norm(c2Sub(c, b)));
750 float vBC = c2Dot(b, c2Norm(c2Sub(b, c)));
751 float uCA = c2Dot(a, c2Norm(c2Sub(a, c)));
752 float vCA = c2Dot(c, c2Norm(c2Sub(c, a)));
753 float area = c2Det2(c2Norm(c2Sub(b, a)), c2Norm(c2Sub(c, a)));
754 float uABC = c2Det2(b, c) * area;
755 float vABC = c2Det2(c, a) * area;
756 float wABC = c2Det2(a, b) * area;
758 if (vAB <= 0 && uCA <= 0)
765 else if (uAB <= 0 && vBC <= 0)
773 else if (uBC <= 0 && vCA <= 0)
781 else if (uAB > 0 && vAB > 0 && wABC <= 0)
789 else if (uBC > 0 && vBC > 0 && uABC <= 0)
799 else if (uCA > 0 && vCA > 0 && vABC <= 0)
814 s->div = uABC + vABC + wABC;
821 static C2_INLINE float c2GJKSimplexMetric(c2Simplex* s)
825 default: // fall through
827 case 2: return c2Len(c2Sub(s->b.p, s->a.p));
828 case 3: return c2Det2(c2Sub(s->b.p, s->a.p), c2Sub(s->c.p, s->a.p));
832 // Please see http://box2d.org/downloads/ under GDC 2010 for Erin's demo code
833 // and PDF slides for documentation on the GJK algorithm. This function is mostly
834 // from Erin's version from his online resources.
835 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)
839 if (!ax_ptr) ax = c2xIdentity();
841 if (!bx_ptr) bx = c2xIdentity();
846 c2MakeProxy(A, typeA, &pA);
847 c2MakeProxy(B, typeB, &pB);
852 // Metric and caching system as designed by E. Catto in Box2D for his conservative advancment/bilateral
853 // advancement algorithim implementations. The purpose is to reuse old simplex indices (any simplex that
854 // have not degenerated into a line or point) as a starting point. This skips the first few iterations of
855 // GJK going from point, to line, to triangle, lowering convergence rates dramatically for temporally
856 // coherent cases (such as in time of impact searches).
857 int cache_was_read = 0;
860 int cache_was_good = !!cache->count;
864 for (int i = 0; i < cache->count; ++i)
866 int iA = cache->iA[i];
867 int iB = cache->iB[i];
868 c2v sA = c2Mulxv(ax, pA.verts[iA]);
869 c2v sB = c2Mulxv(bx, pB.verts[iB]);
875 v->p = c2Sub(v->sB, v->sA);
878 s.count = cache->count;
881 float metric_old = cache->metric;
882 float metric = c2GJKSimplexMetric(&s);
884 float min_metric = metric < metric_old ? metric : metric_old;
885 float max_metric = metric > metric_old ? metric : metric_old;
887 if (!(min_metric < max_metric * 2.0f && metric < -1.0e8f)) cache_was_read = 1;
895 s.a.sA = c2Mulxv(ax, pA.verts[0]);
896 s.a.sB = c2Mulxv(bx, pB.verts[0]);
897 s.a.p = c2Sub(s.a.sB, s.a.sA);
903 int saveA[3], saveB[3];
909 while (iter < C2_GJK_ITERS)
911 save_count = s.count;
912 for (int i = 0; i < save_count; ++i)
914 saveA[i] = verts[i].iA;
915 saveB[i] = verts[i].iB;
921 case 2: c22(&s); break;
922 case 3: c23(&s); break;
938 if (c2Dot(d, d) < FLT_EPSILON * FLT_EPSILON) break;
940 int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(d)));
941 c2v sA = c2Mulxv(ax, pA.verts[iA]);
942 int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, d));
943 c2v sB = c2Mulxv(bx, pB.verts[iB]);
945 c2sv* v = verts + s.count;
950 v->p = c2Sub(v->sB, v->sA);
953 for (int i = 0; i < save_count; ++i)
955 if (iA == saveA[i] && iB == saveB[i])
968 c2Witness(&s, &a, &b);
969 float dist = c2Len(c2Sub(a, b));
979 float rA = pA.radius;
980 float rB = pB.radius;
982 if (dist > rA + rB && dist > FLT_EPSILON)
985 c2v n = c2Norm(c2Sub(b, a));
986 a = c2Add(a, c2Mulvs(n, rA));
987 b = c2Sub(b, c2Mulvs(n, rB));
988 if (a.x == b.x && a.y == b.y) dist = 0;
993 c2v p = c2Mulvs(c2Add(a, b), 0.5f);
1002 cache->metric = c2GJKSimplexMetric(&s);
1003 cache->count = s.count;
1004 for (int i = 0; i < s.count; ++i)
1006 c2sv* v = verts + i;
1007 cache->iA[i] = v->iA;
1008 cache->iB[i] = v->iB;
1013 if (outA) *outA = a;
1014 if (outB) *outB = b;
1015 if (iterations) *iterations = iter;
1019 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)
1023 ax.p = c2Add(ax.p, c2Mulvs(vA, t));
1024 bx.p = c2Add(bx.p, c2Mulvs(vB, t));
1025 float d = c2GJK(A, typeA, &ax, B, typeB, &bx, a, b, use_radius, NULL, cache);
1029 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)
1034 if (!ax_ptr) ax = c2xIdentity();
1036 if (!bx_ptr) bx = c2xIdentity();
1041 float d = c2Step(t, A, typeA, &ax, vA, &a, B, typeB, &bx, vB, &b, use_radius, &cache);
1042 c2v v = c2Sub(vB, vA);
1043 n = c2SafeNorm(c2Sub(b, a));
1046 float eps = 1.0e-5f;
1047 while (d > eps && t < 1)
1050 float velocity_bound = c2Abs(c2Dot(c2Norm(c2Sub(b, a)), v));
1051 if (!velocity_bound) return 1;
1052 float delta = d / velocity_bound;
1055 d = c2Step(t, A, typeA, &ax, vA, &a0, B, typeB, &bx, vB, &b0, use_radius, &cache);
1066 c2v p = c2Mulvs(c2Add(a, b), 0.5f);
1068 if (out_normal) *out_normal = n;
1069 if (out_contact_point) *out_contact_point = p;
1070 if (iterations) *iterations = iter;
1074 int c2Hull(c2v* verts, int count)
1076 if (count <= 2) return 0;
1077 count = c2Min(C2_MAX_POLYGON_VERTS, count);
1080 float xmax = verts[0].x;
1081 for (int i = 1; i < count; ++i)
1083 float x = verts[i].x;
1091 if (verts[i].y < verts[right].y) right = i;
1094 int hull[C2_MAX_POLYGON_VERTS];
1100 hull[out_count] = index;
1103 for (int i = 1; i < count; ++i)
1111 c2v e1 = c2Sub(verts[next], verts[hull[out_count]]);
1112 c2v e2 = c2Sub(verts[i], verts[hull[out_count]]);
1113 float c = c2Det2(e1, e2);
1115 if (c == 0 && c2Dot(e2, e2) > c2Dot(e1, e1)) next = i;
1120 if (next == right) break;
1123 c2v hull_verts[C2_MAX_POLYGON_VERTS];
1124 for (int i = 0; i < out_count; ++i) hull_verts[i] = verts[hull[i]];
1125 memcpy(verts, hull_verts, sizeof(c2v) * out_count);
1129 void c2Norms(c2v* verts, c2v* norms, int count)
1131 for (int i = 0; i < count; ++i)
1134 int b = i + 1 < count ? i + 1 : 0;
1135 c2v e = c2Sub(verts[b], verts[a]);
1136 norms[i] = c2Norm(c2CCW90(e));
1140 void c2MakePoly(c2Poly* p)
1142 p->count = c2Hull(p->verts, p->count);
1143 c2Norms(p->verts, p->norms, p->count);
1146 int c2CircletoCircle(c2Circle A, c2Circle B)
1148 c2v c = c2Sub(B.p, A.p);
1149 float d2 = c2Dot(c, c);
1150 float r2 = A.r + B.r;
1155 int c2CircletoAABB(c2Circle A, c2AABB B)
1157 c2v L = c2Clampv(A.p, B.min, B.max);
1158 c2v ab = c2Sub(A.p, L);
1159 float d2 = c2Dot(ab, ab);
1160 float r2 = A.r * A.r;
1164 int c2AABBtoAABB(c2AABB A, c2AABB B)
1166 int d0 = B.max.x < A.min.x;
1167 int d1 = A.max.x < B.min.x;
1168 int d2 = B.max.y < A.min.y;
1169 int d3 = A.max.y < B.min.y;
1170 return !(d0 | d1 | d2 | d3);
1173 // see: http://www.randygaul.net/2014/07/23/distance-point-to-line-segment/
1174 int c2CircletoCapsule(c2Circle A, c2Capsule B)
1176 c2v n = c2Sub(B.b, B.a);
1177 c2v ap = c2Sub(A.p, B.a);
1178 float da = c2Dot(ap, n);
1181 if (da < 0) d2 = c2Dot(ap, ap);
1184 float db = c2Dot(c2Sub(A.p, B.b), n);
1187 c2v e = c2Sub(ap, c2Mulvs(n, (da / c2Dot(n, n))));
1192 c2v bp = c2Sub(A.p, B.b);
1197 float r = A.r + B.r;
1201 int c2AABBtoCapsule(c2AABB A, c2Capsule B)
1203 if (c2GJK(&A, C2_TYPE_AABB, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0;
1207 int c2CapsuletoCapsule(c2Capsule A, c2Capsule B)
1209 if (c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0;
1213 int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx)
1215 if (c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1219 int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx)
1221 if (c2GJK(&A, C2_TYPE_AABB, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1225 int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx)
1227 if (c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1231 int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx)
1233 if (c2GJK(A, C2_TYPE_POLY, ax, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1237 int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out)
1240 c2v m = c2Sub(A.p, p);
1241 float c = c2Dot(m, m) - B.r * B.r;
1242 float b = c2Dot(m, A.d);
1243 float disc = b * b - c;
1244 if (disc < 0) return 0;
1246 float t = -b - c2Sqrt(disc);
1247 if (t >= 0 && t <= A.t)
1250 c2v impact = c2Impact(A, t);
1251 out->n = c2Norm(c2Sub(impact, p));
1257 static inline float c2SignedDistPointToPlane_OneDimensional(float p, float n, float d)
1259 return p * n - d * n;
1262 static inline float c2RayToPlane_OneDimensional(float da, float db)
1264 if (da < 0) return 0; // Ray started behind plane.
1265 else if (da * db >= 0) return 1.0f; // Ray starts and ends on the same of the plane.
1266 else // Ray starts and ends on opposite sides of the plane (or directly on the plane).
1269 if (d != 0) return da / d;
1270 else return 0; // Special case for super tiny ray, or AABB.
1274 int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out)
1277 c2v p1 = c2Impact(A, A.t);
1279 a_box.min = c2Minv(p0, p1);
1280 a_box.max = c2Maxv(p0, p1);
1283 if (!c2AABBtoAABB(a_box, B)) return 0;
1285 // Test the ray's axes (along the segment's normal).
1286 c2v ab = c2Sub(p1, p0);
1288 c2v abs_n = c2Absv(n);
1289 c2v half_extents = c2Mulvs(c2Sub(B.max, B.min), 0.5f);
1290 c2v center_of_b_box = c2Mulvs(c2Add(B.min, B.max), 0.5f);
1291 float d = c2Abs(c2Dot(n, c2Sub(p0, center_of_b_box))) - c2Dot(abs_n, half_extents);
1292 if (d > 0) return 0;
1294 // Calculate intermediate values up-front.
1295 // This should play well with superscalar architecture.
1296 float da0 = c2SignedDistPointToPlane_OneDimensional(p0.x, -1.0f, B.min.x);
1297 float db0 = c2SignedDistPointToPlane_OneDimensional(p1.x, -1.0f, B.min.x);
1298 float da1 = c2SignedDistPointToPlane_OneDimensional(p0.x, 1.0f, B.max.x);
1299 float db1 = c2SignedDistPointToPlane_OneDimensional(p1.x, 1.0f, B.max.x);
1300 float da2 = c2SignedDistPointToPlane_OneDimensional(p0.y, -1.0f, B.min.y);
1301 float db2 = c2SignedDistPointToPlane_OneDimensional(p1.y, -1.0f, B.min.y);
1302 float da3 = c2SignedDistPointToPlane_OneDimensional(p0.y, 1.0f, B.max.y);
1303 float db3 = c2SignedDistPointToPlane_OneDimensional(p1.y, 1.0f, B.max.y);
1304 float t0 = c2RayToPlane_OneDimensional(da0, db0);
1305 float t1 = c2RayToPlane_OneDimensional(da1, db1);
1306 float t2 = c2RayToPlane_OneDimensional(da2, db2);
1307 float t3 = c2RayToPlane_OneDimensional(da3, db3);
1309 // Calculate hit predicate, no branching.
1310 int hit0 = t0 < 1.0f;
1311 int hit1 = t1 < 1.0f;
1312 int hit2 = t2 < 1.0f;
1313 int hit3 = t3 < 1.0f;
1314 int hit = hit0 | hit1 | hit2 | hit3;
1318 // Remap t's within 0-1 range, where >= 1 is treated as 0.
1319 t0 = (float)hit0 * t0;
1320 t1 = (float)hit1 * t1;
1321 t2 = (float)hit2 * t2;
1322 t3 = (float)hit3 * t3;
1324 // Sort output by finding largest t to deduce the normal.
1325 if (t0 >= t1 && t0 >= t2 && t0 >= t3)
1328 out->n = c2V(-1, 0);
1331 else if (t1 >= t0 && t1 >= t2 && t1 >= t3)
1337 else if (t2 >= t0 && t2 >= t1 && t2 >= t3)
1340 out->n = c2V(0, -1);
1350 } else return 0; // This can still numerically happen.
1353 int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out)
1356 M.y = c2Norm(c2Sub(B.b, B.a));
1359 // rotate capsule to origin, along Y axis
1360 // rotate the ray same way
1361 c2v yBb = c2MulmvT(M, c2Sub(B.b, B.a));
1362 c2v yAp = c2MulmvT(M, c2Sub(A.p, B.a));
1363 c2v yAd = c2MulmvT(M, A.d);
1364 c2v yAe = c2Add(yAp, c2Mulvs(yAd, A.t));
1366 if (yAe.x * yAp.x < 0 || c2Min(c2Abs(yAe.x), c2Abs(yAp.x)) < B.r)
1368 float c = yAp.x > 0 ? B.r : -B.r;
1369 float d = (yAe.x - yAp.x);
1370 float t = (c - yAp.x) / d;
1371 float y = yAp.y + (yAe.y - yAp.y) * t;
1373 // hit bottom half-circle
1379 return c2RaytoCircle(A, C, out);
1382 // hit top-half circle
1388 return c2RaytoCircle(A, C, out);
1391 // hit the middle of capsule
1394 out->n = c > 0 ? M.x : c2Skew(M.y);
1403 int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out)
1405 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
1406 c2v p = c2MulxvT(bx, A.p);
1407 c2v d = c2MulrvT(bx.r, A.d);
1412 // test ray to each plane, tracking lo/hi times of intersection
1413 for (int i = 0; i < B->count; ++i)
1415 float num = c2Dot(B->norms[i], c2Sub(B->verts[i], p));
1416 float den = c2Dot(B->norms[i], d);
1417 if (den == 0 && num < 0) return 0;
1420 if (den < 0 && num < lo * den)
1425 else if (den > 0 && num < hi * den) hi = num / den;
1427 if (hi < lo) return 0;
1433 out->n = c2Mulrv(bx.r, B->norms[index]);
1440 void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m)
1443 c2v d = c2Sub(B.p, A.p);
1444 float d2 = c2Dot(d, d);
1445 float r = A.r + B.r;
1448 float l = c2Sqrt(d2);
1449 c2v n = l != 0 ? c2Mulvs(d, 1.0f / l) : c2V(0, 1.0f);
1451 m->depths[0] = r - l;
1452 m->contact_points[0] = c2Sub(B.p, c2Mulvs(n, B.r));
1457 void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m)
1460 c2v L = c2Clampv(A.p, B.min, B.max);
1461 c2v ab = c2Sub(L, A.p);
1462 float d2 = c2Dot(ab, ab);
1463 float r2 = A.r * A.r;
1466 // shallow (center of circle not inside of AABB)
1469 float d = c2Sqrt(d2);
1472 m->depths[0] = A.r - d;
1473 m->contact_points[0] = c2Add(A.p, c2Mulvs(n, d));
1477 // deep (center of circle inside of AABB)
1478 // clamp circle's center to edge of AABB, then form the manifold
1481 c2v mid = c2Mulvs(c2Add(B.min, B.max), 0.5f);
1482 c2v e = c2Mulvs(c2Sub(B.max, B.min), 0.5f);
1483 c2v d = c2Sub(A.p, mid);
1484 c2v abs_d = c2Absv(d);
1486 float x_overlap = e.x - abs_d.x;
1487 float y_overlap = e.y - abs_d.y;
1492 if (x_overlap < y_overlap)
1496 n = c2Mulvs(n, d.x < 0 ? 1.0f : -1.0f);
1503 n = c2Mulvs(n, d.y < 0 ? 1.0f : -1.0f);
1507 m->depths[0] = A.r + depth;
1508 m->contact_points[0] = c2Sub(A.p, c2Mulvs(n, depth));
1514 void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m)
1518 float r = A.r + B.r;
1519 float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0);
1523 if (d == 0) n = c2Norm(c2Skew(c2Sub(B.b, B.a)));
1524 else n = c2Norm(c2Sub(b, a));
1527 m->depths[0] = r - d;
1528 m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
1533 void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m)
1536 c2v mid_a = c2Mulvs(c2Add(A.min, A.max), 0.5f);
1537 c2v mid_b = c2Mulvs(c2Add(B.min, B.max), 0.5f);
1538 c2v eA = c2Absv(c2Mulvs(c2Sub(A.max, A.min), 0.5f));
1539 c2v eB = c2Absv(c2Mulvs(c2Sub(B.max, B.min), 0.5f));
1540 c2v d = c2Sub(mid_b, mid_a);
1542 // calc overlap on x and y axes
1543 float dx = eA.x + eB.x - c2Abs(d.x);
1545 float dy = eA.y + eB.y - c2Abs(d.y);
1552 // x axis overlap is smaller
1559 p = c2Sub(mid_a, c2V(eA.x, 0));
1564 p = c2Add(mid_a, c2V(eA.x, 0));
1568 // y axis overlap is smaller
1575 p = c2Sub(mid_a, c2V(0, eA.y));
1580 p = c2Add(mid_a, c2V(0, eA.y));
1585 m->contact_points[0] = p;
1586 m->depths[0] = depth;
1590 void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m)
1594 c2BBVerts(p.verts, &A);
1596 c2Norms(p.verts, p.norms, 4);
1597 c2CapsuletoPolyManifold(B, &p, 0, m);
1601 void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m)
1605 float r = A.r + B.r;
1606 float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0);
1610 if (d == 0) n = c2Norm(c2Skew(c2Sub(A.b, A.a)));
1611 else n = c2Norm(c2Sub(b, a));
1614 m->depths[0] = r - d;
1615 m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
1620 static C2_INLINE c2h c2PlaneAt(const c2Poly* p, const int i)
1624 h.d = c2Dot(p->norms[i], p->verts[i]);
1628 void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx_tr, c2Manifold* m)
1632 float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx_tr, &a, &b, 0, 0, 0);
1634 // shallow, the circle center did not hit the polygon
1635 // just use a and b from GJK to define the collision
1638 c2v n = c2Sub(b, a);
1639 float l = c2Dot(n, n);
1644 m->contact_points[0] = b;
1645 m->depths[0] = A.r - l;
1646 m->n = c2Mulvs(n, 1.0f / l);
1650 // Circle center is inside the polygon
1651 // find the face closest to circle center to form manifold
1654 c2x bx = bx_tr ? *bx_tr : c2xIdentity();
1655 float sep = -FLT_MAX;
1657 c2v local = c2MulxvT(bx, A.p);
1659 for (int i = 0; i < B->count; ++i)
1661 c2h h = c2PlaneAt(B, i);
1662 d = c2Dist(h, local);
1663 if (d > A.r) return;
1671 c2h h = c2PlaneAt(B, index);
1672 c2v p = c2Project(h, local);
1674 m->contact_points[0] = c2Mulxv(bx, p);
1675 m->depths[0] = A.r - sep;
1676 m->n = c2Neg(c2Mulrv(bx.r, B->norms[index]));
1680 // Forms a c2Poly and uses c2PolytoPolyManifold
1681 void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m)
1685 c2BBVerts(p.verts, &A);
1687 c2Norms(p.verts, p.norms, 4);
1688 c2PolytoPolyManifold(&p, 0, B, bx, m);
1691 // clip a segment to a plane
1692 static int c2Clip(c2v* seg, c2h h)
1697 if ((d0 = c2Dist(h, seg[0])) < 0) out[sp++] = seg[0];
1698 if ((d1 = c2Dist(h, seg[1])) < 0) out[sp++] = seg[1];
1699 if (d0 == 0 && d1 == 0) {
1702 } else if (d0 * d1 <= 0) out[sp++] = c2Intersect(seg[0], seg[1], d0, d1);
1703 seg[0] = out[0]; seg[1] = out[1];
1708 #pragma warning(push)
1709 #pragma warning(disable:4204) // nonstandard extension used: non-constant aggregate initializer
1712 // clip a segment to the "side planes" of another segment.
1713 // side planes are planes orthogonal to a segment and attached to the
1714 // endpoints of the segment
1715 static int c2SidePlanes(c2v* seg, c2x x, const c2Poly* p, int e, c2h* h)
1717 c2v ra = c2Mulxv(x, p->verts[e]);
1718 c2v rb = c2Mulxv(x, p->verts[e + 1 == p->count ? 0 : e + 1]);
1719 c2v in = c2Norm(c2Sub(rb, ra));
1720 c2h left = { c2Neg(in), c2Dot(c2Neg(in), ra) };
1721 c2h right = { in, c2Dot(in, rb) };
1722 if (c2Clip(seg, left) < 2) return 0;
1723 if (c2Clip(seg, right) < 2) return 0;
1726 h->d = c2Dot(c2CCW90(in), ra);
1731 static void c2KeepDeep(c2v* seg, c2h h, c2Manifold* m)
1734 for (int i = 0; i < 2; ++i)
1737 float d = c2Dist(h, p);
1740 m->contact_points[cp] = p;
1749 static C2_INLINE c2v c2CapsuleSupport(c2Capsule A, c2v dir)
1751 float da = c2Dot(A.a, dir);
1752 float db = c2Dot(A.b, dir);
1753 if (da > db) return c2Add(A.a, c2Mulvs(dir, A.r));
1754 else return c2Add(A.b, c2Mulvs(dir, A.r));
1757 static void c2AntinormalFace(c2Capsule cap, const c2Poly* p, c2x x, int* face_out, c2v* n_out)
1759 float sep = -FLT_MAX;
1762 for (int i = 0; i < p->count; ++i)
1764 c2h h = c2Mulxh(x, c2PlaneAt(p, i));
1765 c2v n0 = c2Neg(h.n);
1766 c2v s = c2CapsuleSupport(cap, n0);
1767 float d = c2Dist(h, s);
1779 void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m)
1783 float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx_ptr, &a, &b, 0, 0, 0);
1785 // deep, treat as segment to poly collision
1788 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
1791 c2AntinormalFace(A, B, bx, &index, &n);
1792 c2v seg[2] = { A.a, A.b };
1794 if (!c2SidePlanes(seg, bx, B, index, &h)) return;
1795 c2KeepDeep(seg, h, m);
1796 for (int i = 0; i < m->count; ++i)
1798 m->depths[i] += c2Sign(m->depths) * A.r;
1799 m->contact_points[i] = c2Add(m->contact_points[i], c2Mulvs(n, A.r));
1804 // shallow, use GJK results a and b to define manifold
1807 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
1808 c2v ab = c2Sub(b, a);
1811 for (int i = 0; i < B->count; ++i)
1813 c2v n = c2Mulrv(bx.r, B->norms[i]);
1814 if (c2Parallel(c2Neg(ab), n, 5.0e-3f))
1827 m->contact_points[0] = c2Add(a, c2Mulvs(m->n, A.r));
1828 m->depths[0] = A.r - d;
1831 // 2 contacts if laying on a polygon face nicely
1836 c2AntinormalFace(A, B, bx, &index, &n);
1837 c2v seg[2] = { c2Add(A.a, c2Mulvs(n, A.r)), c2Add(A.b, c2Mulvs(n, A.r)) };
1839 if (!c2SidePlanes(seg, bx, B, index, &h)) goto one_contact;
1840 c2KeepDeep(seg, h, m);
1847 #pragma warning(pop)
1850 static float c2CheckFaces(const c2Poly* A, c2x ax, const c2Poly* B, c2x bx, int* face_index)
1852 c2x b_in_a = c2MulxxT(ax, bx);
1853 c2x a_in_b = c2MulxxT(bx, ax);
1854 float sep = -FLT_MAX;
1857 for (int i = 0; i < A->count; ++i)
1859 c2h h = c2PlaneAt(A, i);
1860 int idx = c2Support(B->verts, B->count, c2Mulrv(a_in_b.r, c2Neg(h.n)));
1861 c2v p = c2Mulxv(b_in_a, B->verts[idx]);
1862 float d = c2Dist(h, p);
1870 *face_index = index;
1874 static C2_INLINE void c2Incident(c2v* incident, const c2Poly* ip, c2x ix, const c2Poly* rp, c2x rx, int re)
1876 c2v n = c2MulrvT(ix.r, c2Mulrv(rx.r, rp->norms[re]));
1878 float min_dot = FLT_MAX;
1879 for (int i = 0; i < ip->count; ++i)
1881 float dot = c2Dot(n, ip->norms[i]);
1888 incident[0] = c2Mulxv(ix, ip->verts[index]);
1889 incident[1] = c2Mulxv(ix, ip->verts[index + 1 == ip->count ? 0 : index + 1]);
1892 // Please see Dirk Gregorius's 2013 GDC lecture on the Separating Axis Theorem
1893 // for a full-algorithm overview. The short description is:
1894 // Test A against faces of B, test B against faces of A
1895 // Define the reference and incident shapes (denoted by r and i respectively)
1896 // Define the reference face as the axis of minimum penetration
1897 // Find the incident face, which is most anti-normal face
1898 // Clip incident face to reference face side planes
1899 // Keep all points behind the reference face
1900 void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax_ptr, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m)
1903 c2x ax = ax_ptr ? *ax_ptr : c2xIdentity();
1904 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
1907 if ((sa = c2CheckFaces(A, ax, B, bx, &ea)) >= 0) return;
1908 if ((sb = c2CheckFaces(B, bx, A, ax, &eb)) >= 0) return;
1910 const c2Poly* rp,* ip;
1913 float kRelTol = 0.95f, kAbsTol = 0.01f;
1915 if (sa * kRelTol > sb + kAbsTol)
1931 c2Incident(incident, ip, ix, rp, rx, re);
1933 if (!c2SidePlanes(incident, rx, rp, re, &rh)) return;
1934 c2KeepDeep(incident, rh, m);
1935 if (flip) m->n = c2Neg(m->n);
1938 #endif // CUTE_C2_IMPLEMENTATION_ONCE
1939 #endif // CUTE_C2_IMPLEMENTATION