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 between two shapes
47 * c2TOI - computes the time of impact between two shapes, useful for sweeping shapes, or doing shape casts
48 * c2MakePoly - Runs convex hull algorithm and computes normals on input point-set
49 * c2Collided - generic version of c2***to*** funcs
50 * c2Collide - generic version of c2***to***Manifold funcs
51 * c2CastRay - generic version of c2Rayto*** funcs
53 The rest of the header is more or less for internal use. Here is an example of
54 making some shapes and testing for collision:
61 cap.a = first_endpoint;
62 cap.b = second_endpoint;
65 int hit = c2CircletoCapsule(c, cap);
68 handle collision here...
71 For more code examples and tests please see:
72 https://github.com/RandyGaul/cute_header/tree/master/examples_cute_gl_and_c2
74 Here is a past discussion thread on this header:
75 https://www.reddit.com/r/gamedev/comments/5tqyey/tinyc2_2d_collision_detection_library_in_c/
77 Here is a very nice repo containing various tests and examples using SFML for rendering:
78 https://github.com/sro5h/tinyc2-tests
83 * Circles, capsules, AABBs, rays and convex polygons are supported
84 * Fast boolean only result functions (hit yes/no)
85 * Slghtly slower manifold generation for collision normals + depths +points
86 * GJK implementation (finds closest points for disjoint pairs of shapes)
87 * Shape casts/sweeps with c2TOI function (time of impact)
88 * Robust 2D convex hull generator
89 * Lots of correctly implemented and tested 2D math routines
90 * Implemented in portable C, and is readily portable to other languages
91 * Generic c2Collide, c2Collided and c2CastRay function (can pass in any shape type)
92 * Extensive examples at: https://github.com/RandyGaul/cute_headers/tree/master/examples_cute_gl_and_c2
97 1.0 (02/13/2017) initial release
98 1.01 (02/13/2017) const crusade, minor optimizations, capsule degen
99 1.02 (03/21/2017) compile fixes for c on more compilers
100 1.03 (09/15/2017) various bugfixes and quality of life changes to manifolds
101 1.04 (03/25/2018) fixed manifold bug in c2CircletoAABBManifold
102 1.05 (11/01/2018) added c2TOI (time of impact) for shape cast/sweep test
103 1.06 (08/23/2019) C2_*** types to C2_TYPE_***, and CUTE_C2_API
104 1.07 (10/19/2019) Optimizations to c2TOI - breaking change to c2GJK API
105 1.08 (12/22/2019) Remove contact point + normal from c2TOI, removed feather
106 radius from c2GJK, fixed various bugs in capsule to poly
107 manifold, did a pass on all docs
108 1.09 (07/27/2019) Added c2Inflate - to inflate/deflate shapes for c2TOI
109 1.10 (02/05/2022) Implemented GJK-Raycast for c2TOI (from E. Catto's Box2D)
114 Plastburk 1.01 - const pointers pull request
115 mmozeiko 1.02 - 3 compile bugfixes
116 felipefs 1.02 - 3 compile bugfixes
117 seemk 1.02 - fix branching bug in c2Collide
118 sro5h 1.02 - bug reports for multiple manifold funcs
119 sro5h 1.03 - work involving quality of life fixes for manifolds
120 Wizzard033 1.06 - C2_*** types to C2_TYPE_***, and CUTE_C2_API
121 Tyler Glaeil 1.08 - Lots of bug reports and disussion on capsules + TOIs
128 This header does not implement a broad-phase, and instead concerns itself with
129 the narrow-phase. This means this header just checks to see if two individual
130 shapes are touching, and can give information about how they are touching.
132 Very common 2D broad-phases are tree and grid approaches. Quad trees are good
133 for static geometry that does not move much if at all. Dynamic AABB trees are
134 good for general purpose use, and can handle moving objects very well. Grids
135 are great and are similar to quad trees.
137 If implementing a grid it can be wise to have each collideable grid cell hold
138 an integer. This integer refers to a 2D shape that can be passed into the
139 various functions in this header. The shape can be transformed from "model"
140 space to "world" space using c2x -- a transform struct. In this way a grid
141 can be implemented that holds any kind of convex shape (that this header
142 supports) while conserving memory with shape instancing.
146 Many of the functions in cute c2 use `c2GJK`, an implementation of the GJK
147 algorithm. Internally GJK computes signed area values, and these values are
148 very numerically sensitive to large shapes. This means the GJK function will
149 break down if input shapes are too large or too far away from the origin.
151 In general it is best to compute collision detection on small shapes very
152 close to the origin. One trick is to keep your collision information numerically
153 very tiny, and simply scale it up when rendering to the appropriate size.
155 For reference, if your shapes are all AABBs and contain a width and height
156 of somewhere between 1.0f and 10.0f, everything will be fine. However, once
157 your shapes start approaching a width/height of 100.0f to 1000.0f GJK can
160 This is a complicated topic, so feel free to ask the author for advice here.
162 Here is an example demonstrating this problem with two large AABBs:
163 https://github.com/RandyGaul/cute_headers/issues/160
165 Please email at my address with any questions or comments at:
166 author's last name followed by 1748 at gmail
169 #if !defined(CUTE_C2_H)
171 // this can be adjusted as necessary, but is highly recommended to be kept at 8.
172 // higher numbers will incur quite a bit of memory overhead, and convex shapes
173 // over 8 verts start to just look like spheres, which can be implicitly rep-
174 // resented as a point + radius. usually tools that generate polygons should be
175 // constructed so they do not output polygons with too many verts.
176 // Note: polygons in cute_c2 are all *convex*.
177 #define C2_MAX_POLYGON_VERTS 8
186 // 2d rotation composed of cos/sin pair for a single angle
187 // We use two floats as a small optimization to avoid computing sin/cos unnecessarily
194 // 2d rotation matrix
201 // 2d transformation "x"
202 // These are used especially for c2Poly when a c2Poly is passed to a function.
203 // Since polygons are prime for "instancing" a c2x transform can be used to
204 // transform a polygon from local space to world space. In functions that take
205 // a c2x pointer (like c2PolytoPoly), these pointers can be NULL, which represents
206 // an identity transformation and assumes the verts inside of c2Poly are already
214 // 2d halfspace (aka plane, aka line)
217 c2v n; // normal, normalized
218 float d; // distance to origin from plane, or ax + by = d
221 typedef struct c2Circle
227 typedef struct c2AABB
233 // a capsule is defined as a line segment (from a to b) and radius r
234 typedef struct c2Capsule
241 typedef struct c2Poly
244 c2v verts[C2_MAX_POLYGON_VERTS];
245 c2v norms[C2_MAX_POLYGON_VERTS];
249 // Many algorithms in this file are sensitive to the magnitude of the
250 // ray direction (c2Ray::d). It is highly recommended to normalize the
251 // ray direction and use t to specify a distance. Please see this link
252 // for an in-depth explanation: https://github.com/RandyGaul/cute_headers/issues/30
256 c2v d; // direction (normalized)
257 float t; // distance along d from position p to find endpoint of ray
260 typedef struct c2Raycast
262 float t; // time of impact
263 c2v n; // normal of surface at impact (unit length)
266 // position of impact p = ray.p + ray.d * raycast.t
267 #define c2Impact(ray, t) c2Add(ray.p, c2Mulvs(ray.d, t))
269 // contains all information necessary to resolve a collision, or in other words
270 // this is the information needed to separate shapes that are colliding. Doing
271 // the resolution step is *not* included in cute_c2.
272 typedef struct c2Manifold
276 c2v contact_points[2];
278 // always points from shape A to shape B (first and second shapes passed into
279 // any of the c2***to***Manifold functions)
283 // This define allows exporting/importing of the header to a dynamic library.
284 // Here's an example.
285 // #define CUTE_C2_API extern "C" __declspec(dllexport)
286 #if !defined(CUTE_C2_API)
290 // boolean collision detection
291 // these versions are faster than the manifold versions, but only give a YES/NO result
292 CUTE_C2_API int c2CircletoCircle(c2Circle A, c2Circle B);
293 CUTE_C2_API int c2CircletoAABB(c2Circle A, c2AABB B);
294 CUTE_C2_API int c2CircletoCapsule(c2Circle A, c2Capsule B);
295 CUTE_C2_API int c2AABBtoAABB(c2AABB A, c2AABB B);
296 CUTE_C2_API int c2AABBtoCapsule(c2AABB A, c2Capsule B);
297 CUTE_C2_API int c2CapsuletoCapsule(c2Capsule A, c2Capsule B);
298 CUTE_C2_API int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx);
299 CUTE_C2_API int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx);
300 CUTE_C2_API int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx);
301 CUTE_C2_API int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx);
304 // output is placed into the c2Raycast struct, which represents the hit location
305 // of the ray. the out param contains no meaningful information if these funcs
307 CUTE_C2_API int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out);
308 CUTE_C2_API int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out);
309 CUTE_C2_API int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out);
310 CUTE_C2_API int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out);
312 // manifold generation
313 // These functions are (generally) slower than the boolean versions, but will compute one
314 // or two points that represent the plane of contact. This information is usually needed
315 // to resolve and prevent shapes from colliding. If no collision occured the count member
316 // of the manifold struct is set to 0.
317 CUTE_C2_API void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m);
318 CUTE_C2_API void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m);
319 CUTE_C2_API void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m);
320 CUTE_C2_API void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m);
321 CUTE_C2_API void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m);
322 CUTE_C2_API void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m);
323 CUTE_C2_API void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx, c2Manifold* m);
324 CUTE_C2_API void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m);
325 CUTE_C2_API void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx, c2Manifold* m);
326 CUTE_C2_API void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx, c2Manifold* m);
337 // This struct is only for advanced usage of the c2GJK function. See comments inside of the
338 // c2GJK function for more details.
339 typedef struct c2GJKCache
348 // This is an advanced function, intended to be used by people who know what they're doing.
350 // Runs the GJK algorithm to find closest points, returns distance between closest points.
351 // outA and outB can be NULL, in this case only distance is returned. ax_ptr and bx_ptr
352 // can be NULL, and represent local to world transformations for shapes A and B respectively.
353 // use_radius will apply radii for capsules and circles (if set to false, spheres are
354 // treated as points and capsules are treated as line segments i.e. rays). The cache parameter
355 // should be NULL, as it is only for advanced usage (unless you know what you're doing, then
356 // go ahead and use it). iterations is an optional parameter.
359 // The GJK function is sensitive to large shapes, since it internally will compute signed area
360 // values. `c2GJK` is called throughout cute c2 in many ways, so try to make sure all of your
361 // collision shapes are not gigantic. For example, try to keep the volume of all your shapes
362 // less than 100.0f. If you need large shapes, you should use tiny collision geometry for all
363 // cute c2 function, and simply render the geometry larger on-screen by scaling it up.
364 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);
366 // Stores results of a time of impact calculation done by `c2TOI`.
369 int hit; // 1 if shapes were touching at the TOI, 0 if they never hit.
370 float toi; // The time of impact between two shapes.
371 c2v n; // Surface normal from shape A to B at the time of impact.
372 c2v p; // Point of contact between shapes A and B at time of impact.
373 int iterations; // Number of iterations the solver underwent.
376 // This is an advanced function, intended to be used by people who know what they're doing.
378 // Computes the time of impact from shape A and shape B. The velocity of each shape is provided
379 // by vA and vB respectively. The shapes are *not* allowed to rotate over time. The velocity is
380 // assumed to represent the change in motion from time 0 to time 1, and so the return value will
381 // be a number from 0 to 1. To move each shape to the colliding configuration, multiply vA and vB
382 // each by the return value. ax_ptr and bx_ptr are optional parameters to transforms for each shape,
383 // and are typically used for polygon shapes to transform from model to world space. Set these to
384 // NULL to represent identity transforms. iterations is an optional parameter. use_radius
385 // will apply radii for capsules and circles (if set to false, spheres are treated as points and
386 // capsules are treated as line segments i.e. rays).
389 // The c2TOI function can be used to implement a "swept character controller", but it can be
390 // difficult to do so. Say we compute a time of impact with `c2TOI` and move the shapes to the
391 // time of impact, and adjust the velocity by zeroing out the velocity along the surface normal.
392 // If we then call `c2TOI` again, it will fail since the shapes will be considered to start in
393 // a colliding configuration. There are many styles of tricks to get around this problem, and
394 // all of them involve giving the next call to `c2TOI` some breathing room. It is recommended
395 // to use some variation of the following algorithm:
398 // 2. Move the shapes to the TOI.
399 // 3. Slightly inflate the size of one, or both, of the shapes so they will be intersecting.
400 // The purpose is to make the shapes numerically intersecting, but not visually intersecting.
401 // Another option is to call c2TOI with slightly deflated shapes.
402 // See the function `c2Inflate` for some more details.
403 // 4. Compute the collision manifold between the inflated shapes (for example, use c2PolytoPolyManifold).
404 // 5. Gently push the shapes apart. This will give the next call to c2TOI some breathing room.
405 CUTE_C2_API c2TOIResult 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);
407 // Inflating a shape.
409 // This is useful to numerically grow or shrink a polytope. For example, when calling
410 // a time of impact function it can be good to use a slightly smaller shape. Then, once
411 // both shapes are moved to the time of impact a collision manifold can be made from the
412 // slightly larger (and now overlapping) shapes.
415 // Inflating a shape with sharp corners can cause those corners to move dramatically.
416 // Deflating a shape can avoid this problem, but deflating a very small shape can invert
417 // the planes and result in something that is no longer convex. Make sure to pick an
418 // appropriately small skin factor, for example 1.0e-6f.
419 CUTE_C2_API void c2Inflate(void* shape, C2_TYPE type, float skin_factor);
421 // Computes 2D convex hull. Will not do anything if less than two verts supplied. If
422 // more than C2_MAX_POLYGON_VERTS are supplied extras are ignored.
423 CUTE_C2_API int c2Hull(c2v* verts, int count);
424 CUTE_C2_API void c2Norms(c2v* verts, c2v* norms, int count);
426 // runs c2Hull and c2Norms, assumes p->verts and p->count are both set to valid values
427 CUTE_C2_API void c2MakePoly(c2Poly* p);
429 // Generic collision detection routines, useful for games that want to use some poly-
430 // morphism to write more generic-styled code. Internally calls various above functions.
431 // For AABBs/Circles/Capsules ax and bx are ignored. For polys ax and bx can define
432 // model to world transformations (for polys only), or be NULL for identity transforms.
433 CUTE_C2_API int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB);
434 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);
435 CUTE_C2_API int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out);
438 #define C2_INLINE __forceinline
440 #define C2_INLINE inline __attribute__((always_inline))
443 // adjust these primitives as seen fit
444 #include <string.h> // memcpy
446 #define c2Sin(radians) sinf(radians)
447 #define c2Cos(radians) cosf(radians)
448 #define c2Sqrt(a) sqrtf(a)
449 #define c2Min(a, b) ((a) < (b) ? (a) : (b))
450 #define c2Max(a, b) ((a) > (b) ? (a) : (b))
451 #define c2Abs(a) ((a) < 0 ? -(a) : (a))
452 #define c2Clamp(a, lo, hi) c2Max(lo, c2Min(a, hi))
453 C2_INLINE void c2SinCos(float radians, float* s, float* c) { *c = c2Cos(radians); *s = c2Sin(radians); }
454 #define c2Sign(a) (a < 0 ? -1.0f : 1.0f)
456 // The rest of the functions in the header-only portion are all for internal use
457 // and use the author's personal naming conventions. It is recommended to use one's
458 // own math library instead of the one embedded here in cute_c2, but for those
459 // curious or interested in trying it out here's the details:
461 // The Mul functions are used to perform multiplication. x stands for transform,
462 // v stands for vector, s stands for scalar, r stands for rotation, h stands for
463 // halfspace and T stands for transpose.For example c2MulxvT stands for "multiply
464 // a transform with a vector, and transpose the transform".
467 C2_INLINE c2v c2V(float x, float y) { c2v a; a.x = x; a.y = y; return a; }
468 C2_INLINE c2v c2Add(c2v a, c2v b) { a.x += b.x; a.y += b.y; return a; }
469 C2_INLINE c2v c2Sub(c2v a, c2v b) { a.x -= b.x; a.y -= b.y; return a; }
470 C2_INLINE float c2Dot(c2v a, c2v b) { return a.x * b.x + a.y * b.y; }
471 C2_INLINE c2v c2Mulvs(c2v a, float b) { a.x *= b; a.y *= b; return a; }
472 C2_INLINE c2v c2Mulvv(c2v a, c2v b) { a.x *= b.x; a.y *= b.y; return a; }
473 C2_INLINE c2v c2Div(c2v a, float b) { return c2Mulvs(a, 1.0f / b); }
474 C2_INLINE c2v c2Skew(c2v a) { c2v b; b.x = -a.y; b.y = a.x; return b; }
475 C2_INLINE c2v c2CCW90(c2v a) { c2v b; b.x = a.y; b.y = -a.x; return b; }
476 C2_INLINE float c2Det2(c2v a, c2v b) { return a.x * b.y - a.y * b.x; }
477 C2_INLINE c2v c2Minv(c2v a, c2v b) { return c2V(c2Min(a.x, b.x), c2Min(a.y, b.y)); }
478 C2_INLINE c2v c2Maxv(c2v a, c2v b) { return c2V(c2Max(a.x, b.x), c2Max(a.y, b.y)); }
479 C2_INLINE c2v c2Clampv(c2v a, c2v lo, c2v hi) { return c2Maxv(lo, c2Minv(a, hi)); }
480 C2_INLINE c2v c2Absv(c2v a) { return c2V(c2Abs(a.x), c2Abs(a.y)); }
481 C2_INLINE float c2Hmin(c2v a) { return c2Min(a.x, a.y); }
482 C2_INLINE float c2Hmax(c2v a) { return c2Max(a.x, a.y); }
483 C2_INLINE float c2Len(c2v a) { return c2Sqrt(c2Dot(a, a)); }
484 C2_INLINE c2v c2Norm(c2v a) { return c2Div(a, c2Len(a)); }
485 C2_INLINE c2v c2SafeNorm(c2v a) { float sq = c2Dot(a, a); return sq ? c2Div(a, c2Len(a)) : c2V(0, 0); }
486 C2_INLINE c2v c2Neg(c2v a) { return c2V(-a.x, -a.y); }
487 C2_INLINE c2v c2Lerp(c2v a, c2v b, float t) { return c2Add(a, c2Mulvs(c2Sub(b, a), t)); }
488 C2_INLINE int c2Parallel(c2v a, c2v b, float kTol)
490 float k = c2Len(a) / c2Len(b);
492 if (c2Abs(a.x - b.x) < kTol && c2Abs(a.y - b.y) < kTol) return 1;
497 C2_INLINE c2r c2Rot(float radians) { c2r r; c2SinCos(radians, &r.s, &r.c); return r; }
498 C2_INLINE c2r c2RotIdentity(void) { c2r r; r.c = 1.0f; r.s = 0; return r; }
499 C2_INLINE c2v c2RotX(c2r r) { return c2V(r.c, r.s); }
500 C2_INLINE c2v c2RotY(c2r r) { return c2V(-r.s, r.c); }
501 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); }
502 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); }
503 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; }
504 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; }
506 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; }
507 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; }
508 C2_INLINE c2m c2Mulmm(c2m a, c2m b) { c2m c; c.x = c2Mulmv(a, b.x); c.y = c2Mulmv(a, b.y); return c; }
509 C2_INLINE c2m c2MulmmT(c2m a, c2m b) { c2m c; c.x = c2MulmvT(a, b.x); c.y = c2MulmvT(a, b.y); return c; }
512 C2_INLINE c2x c2xIdentity(void) { c2x x; x.p = c2V(0, 0); x.r = c2RotIdentity(); return x; }
513 C2_INLINE c2v c2Mulxv(c2x a, c2v b) { return c2Add(c2Mulrv(a.r, b), a.p); }
514 C2_INLINE c2v c2MulxvT(c2x a, c2v b) { return c2MulrvT(a.r, c2Sub(b, a.p)); }
515 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; }
516 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; }
517 C2_INLINE c2x c2Transform(c2v p, float radians) { c2x x; x.r = c2Rot(radians); x.p = p; return x; }
520 C2_INLINE c2v c2Origin(c2h h) { return c2Mulvs(h.n, h.d); }
521 C2_INLINE float c2Dist(c2h h, c2v p) { return c2Dot(h.n, p) - h.d; }
522 C2_INLINE c2v c2Project(c2h h, c2v p) { return c2Sub(p, c2Mulvs(h.n, c2Dist(h, p))); }
523 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; }
524 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; }
525 C2_INLINE c2v c2Intersect(c2v a, c2v b, float da, float db) { return c2Add(a, c2Mulvs(c2Sub(b, a), (da / (da - db)))); }
527 C2_INLINE void c2BBVerts(c2v* out, c2AABB* bb)
530 out[1] = c2V(bb->max.x, bb->min.y);
532 out[3] = c2V(bb->min.x, bb->max.y);
538 #ifdef CUTE_C2_IMPLEMENTATION
539 #ifndef CUTE_C2_IMPLEMENTATION_ONCE
540 #define CUTE_C2_IMPLEMENTATION_ONCE
542 int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB)
549 case C2_TYPE_CIRCLE: return c2CircletoCircle(*(c2Circle*)A, *(c2Circle*)B);
550 case C2_TYPE_AABB: return c2CircletoAABB(*(c2Circle*)A, *(c2AABB*)B);
551 case C2_TYPE_CAPSULE: return c2CircletoCapsule(*(c2Circle*)A, *(c2Capsule*)B);
552 case C2_TYPE_POLY: return c2CircletoPoly(*(c2Circle*)A, (const c2Poly*)B, bx);
560 case C2_TYPE_CIRCLE: return c2CircletoAABB(*(c2Circle*)B, *(c2AABB*)A);
561 case C2_TYPE_AABB: return c2AABBtoAABB(*(c2AABB*)A, *(c2AABB*)B);
562 case C2_TYPE_CAPSULE: return c2AABBtoCapsule(*(c2AABB*)A, *(c2Capsule*)B);
563 case C2_TYPE_POLY: return c2AABBtoPoly(*(c2AABB*)A, (const c2Poly*)B, bx);
568 case C2_TYPE_CAPSULE:
571 case C2_TYPE_CIRCLE: return c2CircletoCapsule(*(c2Circle*)B, *(c2Capsule*)A);
572 case C2_TYPE_AABB: return c2AABBtoCapsule(*(c2AABB*)B, *(c2Capsule*)A);
573 case C2_TYPE_CAPSULE: return c2CapsuletoCapsule(*(c2Capsule*)A, *(c2Capsule*)B);
574 case C2_TYPE_POLY: return c2CapsuletoPoly(*(c2Capsule*)A, (const c2Poly*)B, bx);
582 case C2_TYPE_CIRCLE: return c2CircletoPoly(*(c2Circle*)B, (const c2Poly*)A, ax);
583 case C2_TYPE_AABB: return c2AABBtoPoly(*(c2AABB*)B, (const c2Poly*)A, ax);
584 case C2_TYPE_CAPSULE: return c2CapsuletoPoly(*(c2Capsule*)B, (const c2Poly*)A, ax);
585 case C2_TYPE_POLY: return c2PolytoPoly((const c2Poly*)A, ax, (const c2Poly*)B, bx);
595 void c2Collide(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB, c2Manifold* m)
604 case C2_TYPE_CIRCLE: c2CircletoCircleManifold(*(c2Circle*)A, *(c2Circle*)B, m); break;
605 case C2_TYPE_AABB: c2CircletoAABBManifold(*(c2Circle*)A, *(c2AABB*)B, m); break;
606 case C2_TYPE_CAPSULE: c2CircletoCapsuleManifold(*(c2Circle*)A, *(c2Capsule*)B, m); break;
607 case C2_TYPE_POLY: c2CircletoPolyManifold(*(c2Circle*)A, (const c2Poly*)B, bx, m); break;
614 case C2_TYPE_CIRCLE: c2CircletoAABBManifold(*(c2Circle*)B, *(c2AABB*)A, m); m->n = c2Neg(m->n); break;
615 case C2_TYPE_AABB: c2AABBtoAABBManifold(*(c2AABB*)A, *(c2AABB*)B, m); break;
616 case C2_TYPE_CAPSULE: c2AABBtoCapsuleManifold(*(c2AABB*)A, *(c2Capsule*)B, m); break;
617 case C2_TYPE_POLY: c2AABBtoPolyManifold(*(c2AABB*)A, (const c2Poly*)B, bx, m); break;
621 case C2_TYPE_CAPSULE:
624 case C2_TYPE_CIRCLE: c2CircletoCapsuleManifold(*(c2Circle*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
625 case C2_TYPE_AABB: c2AABBtoCapsuleManifold(*(c2AABB*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
626 case C2_TYPE_CAPSULE: c2CapsuletoCapsuleManifold(*(c2Capsule*)A, *(c2Capsule*)B, m); break;
627 case C2_TYPE_POLY: c2CapsuletoPolyManifold(*(c2Capsule*)A, (const c2Poly*)B, bx, m); break;
634 case C2_TYPE_CIRCLE: c2CircletoPolyManifold(*(c2Circle*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
635 case C2_TYPE_AABB: c2AABBtoPolyManifold(*(c2AABB*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
636 case C2_TYPE_CAPSULE: c2CapsuletoPolyManifold(*(c2Capsule*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
637 case C2_TYPE_POLY: c2PolytoPolyManifold((const c2Poly*)A, ax, (const c2Poly*)B, bx, m); break;
643 int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out)
647 case C2_TYPE_CIRCLE: return c2RaytoCircle(A, *(c2Circle*)B, out);
648 case C2_TYPE_AABB: return c2RaytoAABB(A, *(c2AABB*)B, out);
649 case C2_TYPE_CAPSULE: return c2RaytoCapsule(A, *(c2Capsule*)B, out);
650 case C2_TYPE_POLY: return c2RaytoPoly(A, (const c2Poly*)B, bx, out);
656 #define C2_GJK_ITERS 20
662 c2v verts[C2_MAX_POLYGON_VERTS];
682 static C2_INLINE void c2MakeProxy(const void* shape, C2_TYPE type, c2Proxy* p)
688 c2Circle* c = (c2Circle*)shape;
696 c2AABB* bb = (c2AABB*)shape;
699 c2BBVerts(p->verts, bb);
702 case C2_TYPE_CAPSULE:
704 c2Capsule* c = (c2Capsule*)shape;
713 c2Poly* poly = (c2Poly*)shape;
715 p->count = poly->count;
716 for (int i = 0; i < p->count; ++i) p->verts[i] = poly->verts[i];
721 static C2_INLINE int c2Support(const c2v* verts, int count, c2v d)
724 float dmax = c2Dot(verts[0], d);
726 for (int i = 1; i < count; ++i)
728 float dot = c2Dot(verts[i], d);
739 #define C2_BARY(n, x) c2Mulvs(s->n.x, (den * s->n.u))
740 #define C2_BARY2(x) c2Add(C2_BARY(a, x), C2_BARY(b, x))
741 #define C2_BARY3(x) c2Add(c2Add(C2_BARY(a, x), C2_BARY(b, x)), C2_BARY(c, x))
743 static C2_INLINE c2v c2L(c2Simplex* s)
745 float den = 1.0f / s->div;
748 case 1: return s->a.p;
749 case 2: return C2_BARY2(p);
750 default: return c2V(0, 0);
754 static C2_INLINE void c2Witness(c2Simplex* s, c2v* a, c2v* b)
756 float den = 1.0f / s->div;
759 case 1: *a = s->a.sA; *b = s->a.sB; break;
760 case 2: *a = C2_BARY2(sA); *b = C2_BARY2(sB); break;
761 case 3: *a = C2_BARY3(sA); *b = C2_BARY3(sB); break;
762 default: *a = c2V(0, 0); *b = c2V(0, 0);
766 static C2_INLINE c2v c2D(c2Simplex* s)
770 case 1: return c2Neg(s->a.p);
773 c2v ab = c2Sub(s->b.p, s->a.p);
774 if (c2Det2(ab, c2Neg(s->a.p)) > 0) return c2Skew(ab);
778 default: return c2V(0, 0);
782 static C2_INLINE void c22(c2Simplex* s)
786 float u = c2Dot(b, c2Sub(b, a));
787 float v = c2Dot(a, c2Sub(a, b));
813 static C2_INLINE void c23(c2Simplex* s)
819 float uAB = c2Dot(b, c2Sub(b, a));
820 float vAB = c2Dot(a, c2Sub(a, b));
821 float uBC = c2Dot(c, c2Sub(c, b));
822 float vBC = c2Dot(b, c2Sub(b, c));
823 float uCA = c2Dot(a, c2Sub(a, c));
824 float vCA = c2Dot(c, c2Sub(c, a));
825 float area = c2Det2(c2Sub(b, a), c2Sub(c, a));
826 float uABC = c2Det2(b, c) * area;
827 float vABC = c2Det2(c, a) * area;
828 float wABC = c2Det2(a, b) * area;
830 if (vAB <= 0 && uCA <= 0)
837 else if (uAB <= 0 && vBC <= 0)
845 else if (uBC <= 0 && vCA <= 0)
853 else if (uAB > 0 && vAB > 0 && wABC <= 0)
861 else if (uBC > 0 && vBC > 0 && uABC <= 0)
871 else if (uCA > 0 && vCA > 0 && vABC <= 0)
886 s->div = uABC + vABC + wABC;
893 static C2_INLINE float c2GJKSimplexMetric(c2Simplex* s)
897 default: // fall through
899 case 2: return c2Len(c2Sub(s->b.p, s->a.p));
900 case 3: return c2Det2(c2Sub(s->b.p, s->a.p), c2Sub(s->c.p, s->a.p));
904 // Please see http://box2d.org/downloads/ under GDC 2010 for Erin's demo code
905 // and PDF slides for documentation on the GJK algorithm. This function is mostly
906 // from Erin's version from his online resources.
907 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)
911 if (!ax_ptr) ax = c2xIdentity();
913 if (!bx_ptr) bx = c2xIdentity();
918 c2MakeProxy(A, typeA, &pA);
919 c2MakeProxy(B, typeB, &pB);
924 // Metric and caching system as designed by E. Catto in Box2D for his conservative advancment/bilateral
925 // advancement algorithim implementations. The purpose is to reuse old simplex indices (any simplex that
926 // have not degenerated into a line or point) as a starting point. This skips the first few iterations of
927 // GJK going from point, to line, to triangle, lowering convergence rates dramatically for temporally
928 // coherent cases (such as in time of impact searches).
929 int cache_was_read = 0;
932 int cache_was_good = !!cache->count;
936 for (int i = 0; i < cache->count; ++i)
938 int iA = cache->iA[i];
939 int iB = cache->iB[i];
940 c2v sA = c2Mulxv(ax, pA.verts[iA]);
941 c2v sB = c2Mulxv(bx, pB.verts[iB]);
947 v->p = c2Sub(v->sB, v->sA);
950 s.count = cache->count;
953 float metric_old = cache->metric;
954 float metric = c2GJKSimplexMetric(&s);
956 float min_metric = metric < metric_old ? metric : metric_old;
957 float max_metric = metric > metric_old ? metric : metric_old;
959 if (!(min_metric < max_metric * 2.0f && metric < -1.0e8f)) cache_was_read = 1;
967 s.a.sA = c2Mulxv(ax, pA.verts[0]);
968 s.a.sB = c2Mulxv(bx, pB.verts[0]);
969 s.a.p = c2Sub(s.a.sB, s.a.sA);
975 int saveA[3], saveB[3];
981 while (iter < C2_GJK_ITERS)
983 save_count = s.count;
984 for (int i = 0; i < save_count; ++i)
986 saveA[i] = verts[i].iA;
987 saveB[i] = verts[i].iB;
993 case 2: c22(&s); break;
994 case 3: c23(&s); break;
1010 if (c2Dot(d, d) < FLT_EPSILON * FLT_EPSILON) break;
1012 int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(d)));
1013 c2v sA = c2Mulxv(ax, pA.verts[iA]);
1014 int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, d));
1015 c2v sB = c2Mulxv(bx, pB.verts[iB]);
1017 c2sv* v = verts + s.count;
1022 v->p = c2Sub(v->sB, v->sA);
1025 for (int i = 0; i < save_count; ++i)
1027 if (iA == saveA[i] && iB == saveB[i])
1040 c2Witness(&s, &a, &b);
1041 float dist = c2Len(c2Sub(a, b));
1049 else if (use_radius)
1051 float rA = pA.radius;
1052 float rB = pB.radius;
1054 if (dist > rA + rB && dist > FLT_EPSILON)
1057 c2v n = c2Norm(c2Sub(b, a));
1058 a = c2Add(a, c2Mulvs(n, rA));
1059 b = c2Sub(b, c2Mulvs(n, rB));
1060 if (a.x == b.x && a.y == b.y) dist = 0;
1065 c2v p = c2Mulvs(c2Add(a, b), 0.5f);
1074 cache->metric = c2GJKSimplexMetric(&s);
1075 cache->count = s.count;
1076 for (int i = 0; i < s.count; ++i)
1078 c2sv* v = verts + i;
1079 cache->iA[i] = v->iA;
1080 cache->iB[i] = v->iB;
1085 if (outA) *outA = a;
1086 if (outB) *outB = b;
1087 if (iterations) *iterations = iter;
1091 // Referenced from Box2D's b2ShapeCast function.
1092 // GJK-Raycast algorithm by Gino van den Bergen.
1093 // "Smooth Mesh Contacts with GJK" in Game Physics Pearls, 2010.
1094 c2TOIResult 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)
1099 if (!ax_ptr) ax = c2xIdentity();
1101 if (!bx_ptr) bx = c2xIdentity();
1106 c2MakeProxy(A, typeA, &pA);
1107 c2MakeProxy(B, typeB, &pB);
1113 c2v rv = c2Sub(vB, vA);
1114 int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(rv)));
1115 c2v sA = c2Mulxv(ax, pA.verts[iA]);
1116 int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, rv));
1117 c2v sB = c2Mulxv(bx, pB.verts[iB]);
1118 c2v v = c2Sub(sA, sB);
1120 float rA = pA.radius;
1121 float rB = pB.radius;
1122 float radius = rA + rB;
1128 float tolerance = 1.0e-4f;
1132 result.n = c2V(0, 0);
1133 result.p = c2V(0, 0);
1135 result.iterations = 0;
1137 while (result.iterations < 20 && c2Len(v) - radius > tolerance)
1139 iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(v)));
1140 sA = c2Mulxv(ax, pA.verts[iA]);
1141 iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, v));
1142 sB = c2Mulxv(bx, pB.verts[iB]);
1143 c2v p = c2Sub(sA, sB);
1145 float vp = c2Dot(v, p) - radius;
1146 float vr = c2Dot(v, rv);
1148 if (vr <= 0) return result;
1150 if (t > 1.0f) return result;
1151 result.n = c2Neg(v);
1155 c2sv* sv = verts + s.count;
1157 sv->sA = c2Add(sB, c2Mulvs(rv, t));
1160 sv->p = c2Sub(sv->sB, sv->sA);
1166 case 2: c22(&s); break;
1167 case 3: c23(&s); break;
1175 result.iterations++;
1178 if (result.iterations == 0) {
1181 result.n = c2SafeNorm(c2Neg(v));
1182 int i = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, result.n));
1183 c2v p = c2Mulxv(ax, pA.verts[i]);
1184 p = c2Add(c2Add(p, c2Mulvs(result.n, rA)), c2Mulvs(vA, t));
1193 int c2Hull(c2v* verts, int count)
1195 if (count <= 2) return 0;
1196 count = c2Min(C2_MAX_POLYGON_VERTS, count);
1199 float xmax = verts[0].x;
1200 for (int i = 1; i < count; ++i)
1202 float x = verts[i].x;
1210 if (verts[i].y < verts[right].y) right = i;
1213 int hull[C2_MAX_POLYGON_VERTS];
1219 hull[out_count] = index;
1222 for (int i = 1; i < count; ++i)
1230 c2v e1 = c2Sub(verts[next], verts[hull[out_count]]);
1231 c2v e2 = c2Sub(verts[i], verts[hull[out_count]]);
1232 float c = c2Det2(e1, e2);
1234 if (c == 0 && c2Dot(e2, e2) > c2Dot(e1, e1)) next = i;
1239 if (next == right) break;
1242 c2v hull_verts[C2_MAX_POLYGON_VERTS];
1243 for (int i = 0; i < out_count; ++i) hull_verts[i] = verts[hull[i]];
1244 memcpy(verts, hull_verts, sizeof(c2v) * out_count);
1248 void c2Norms(c2v* verts, c2v* norms, int count)
1250 for (int i = 0; i < count; ++i)
1253 int b = i + 1 < count ? i + 1 : 0;
1254 c2v e = c2Sub(verts[b], verts[a]);
1255 norms[i] = c2Norm(c2CCW90(e));
1259 void c2MakePoly(c2Poly* p)
1261 p->count = c2Hull(p->verts, p->count);
1262 c2Norms(p->verts, p->norms, p->count);
1265 c2Poly c2Dual(c2Poly poly, float skin_factor)
1268 dual.count = poly.count;
1270 // Each plane maps to a point by involution (the mapping is its own inverse) by dividing
1271 // the plane normal by its offset factor.
1272 // plane = a * x + b * y - d
1273 // dual = { a / d, b / d }
1274 for (int i = 0; i < poly.count; ++i) {
1275 c2v n = poly.norms[i];
1276 float d = c2Dot(n, poly.verts[i]) - skin_factor;
1277 if (d == 0) dual.verts[i] = c2V(0, 0);
1278 else dual.verts[i] = c2Div(n, d);
1281 // Instead of canonically building the convex hull, can simply take advantage of how
1282 // the vertices are still in proper CCW order, so only the normals must be recomputed.
1283 c2Norms(dual.verts, dual.norms, dual.count);
1288 // Inflating a polytope, idea by Dirk Gregorius ~ 2015. Works in both 2D and 3D.
1289 // Reference: Halfspace intersection with Qhull by Brad Barber
1290 // http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/half.html
1293 // 1. Find a point within the input poly.
1294 // 2. Center this point onto the origin.
1295 // 3. Adjust the planes by a skin factor.
1296 // 4. Compute the dual vert of each plane. Each plane becomes a vertex.
1297 // c2v dual(c2h plane) { return c2V(plane.n.x / plane.d, plane.n.y / plane.d) }
1298 // 5. Compute the convex hull of the dual verts. This is called the dual.
1299 // 6. Compute the dual of the dual, this will be the poly to return.
1300 // 7. Translate the poly away from the origin by the center point from step 2.
1301 // 8. Return the inflated poly.
1302 c2Poly c2InflatePoly(c2Poly poly, float skin_factor)
1304 c2v average = poly.verts[0];
1305 for (int i = 1; i < poly.count; ++i) {
1306 average = c2Add(average, poly.verts[i]);
1308 average = c2Div(average, (float)poly.count);
1310 for (int i = 0; i < poly.count; ++i) {
1311 poly.verts[i] = c2Sub(poly.verts[i], average);
1314 c2Poly dual = c2Dual(poly, skin_factor);
1315 poly = c2Dual(dual, 0);
1317 for (int i = 0; i < poly.count; ++i) {
1318 poly.verts[i] = c2Add(poly.verts[i], average);
1324 void c2Inflate(void* shape, C2_TYPE type, float skin_factor)
1328 case C2_TYPE_CIRCLE:
1330 c2Circle* circle = (c2Circle*)shape;
1331 circle->r += skin_factor;
1336 c2AABB* bb = (c2AABB*)shape;
1337 c2v factor = c2V(skin_factor, skin_factor);
1338 bb->min = c2Sub(bb->min, factor);
1339 bb->max = c2Add(bb->max, factor);
1342 case C2_TYPE_CAPSULE:
1344 c2Capsule* capsule = (c2Capsule*)shape;
1345 capsule->r += skin_factor;
1350 c2Poly* poly = (c2Poly*)shape;
1351 *poly = c2InflatePoly(*poly, skin_factor);
1356 int c2CircletoCircle(c2Circle A, c2Circle B)
1358 c2v c = c2Sub(B.p, A.p);
1359 float d2 = c2Dot(c, c);
1360 float r2 = A.r + B.r;
1365 int c2CircletoAABB(c2Circle A, c2AABB B)
1367 c2v L = c2Clampv(A.p, B.min, B.max);
1368 c2v ab = c2Sub(A.p, L);
1369 float d2 = c2Dot(ab, ab);
1370 float r2 = A.r * A.r;
1374 int c2AABBtoAABB(c2AABB A, c2AABB B)
1376 int d0 = B.max.x < A.min.x;
1377 int d1 = A.max.x < B.min.x;
1378 int d2 = B.max.y < A.min.y;
1379 int d3 = A.max.y < B.min.y;
1380 return !(d0 | d1 | d2 | d3);
1383 int c2AABBtoPoint(c2AABB A, c2v B)
1385 int d0 = B.x < A.min.x;
1386 int d1 = B.y < A.min.y;
1387 int d2 = B.x > A.max.x;
1388 int d3 = B.y > A.max.y;
1389 return !(d0 | d1 | d2 | d3);
1392 int c2CircleToPoint(c2Circle A, c2v B)
1394 c2v n = c2Sub(A.p, B);
1395 float d2 = c2Dot(n, n);
1396 return d2 < A.r * A.r;
1399 // see: http://www.randygaul.net/2014/07/23/distance-point-to-line-segment/
1400 int c2CircletoCapsule(c2Circle A, c2Capsule B)
1402 c2v n = c2Sub(B.b, B.a);
1403 c2v ap = c2Sub(A.p, B.a);
1404 float da = c2Dot(ap, n);
1407 if (da < 0) d2 = c2Dot(ap, ap);
1410 float db = c2Dot(c2Sub(A.p, B.b), n);
1413 c2v e = c2Sub(ap, c2Mulvs(n, (da / c2Dot(n, n))));
1418 c2v bp = c2Sub(A.p, B.b);
1423 float r = A.r + B.r;
1427 int c2AABBtoCapsule(c2AABB A, c2Capsule B)
1429 if (c2GJK(&A, C2_TYPE_AABB, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0;
1433 int c2CapsuletoCapsule(c2Capsule A, c2Capsule B)
1435 if (c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0;
1439 int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx)
1441 if (c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1445 int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx)
1447 if (c2GJK(&A, C2_TYPE_AABB, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1451 int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx)
1453 if (c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1457 int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx)
1459 if (c2GJK(A, C2_TYPE_POLY, ax, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
1463 int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out)
1466 c2v m = c2Sub(A.p, p);
1467 float c = c2Dot(m, m) - B.r * B.r;
1468 float b = c2Dot(m, A.d);
1469 float disc = b * b - c;
1470 if (disc < 0) return 0;
1472 float t = -b - c2Sqrt(disc);
1473 if (t >= 0 && t <= A.t)
1476 c2v impact = c2Impact(A, t);
1477 out->n = c2Norm(c2Sub(impact, p));
1483 static inline float c2SignedDistPointToPlane_OneDimensional(float p, float n, float d)
1485 return p * n - d * n;
1488 static inline float c2RayToPlane_OneDimensional(float da, float db)
1490 if (da < 0) return 0; // Ray started behind plane.
1491 else if (da * db >= 0) return 1.0f; // Ray starts and ends on the same of the plane.
1492 else // Ray starts and ends on opposite sides of the plane (or directly on the plane).
1495 if (d != 0) return da / d;
1496 else return 0; // Special case for super tiny ray, or AABB.
1500 int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out)
1503 c2v p1 = c2Impact(A, A.t);
1505 a_box.min = c2Minv(p0, p1);
1506 a_box.max = c2Maxv(p0, p1);
1509 if (!c2AABBtoAABB(a_box, B)) return 0;
1511 // Test the ray's axes (along the segment's normal).
1512 c2v ab = c2Sub(p1, p0);
1514 c2v abs_n = c2Absv(n);
1515 c2v half_extents = c2Mulvs(c2Sub(B.max, B.min), 0.5f);
1516 c2v center_of_b_box = c2Mulvs(c2Add(B.min, B.max), 0.5f);
1517 float d = c2Abs(c2Dot(n, c2Sub(p0, center_of_b_box))) - c2Dot(abs_n, half_extents);
1518 if (d > 0) return 0;
1520 // Calculate intermediate values up-front.
1521 // This should play well with superscalar architecture.
1522 float da0 = c2SignedDistPointToPlane_OneDimensional(p0.x, -1.0f, B.min.x);
1523 float db0 = c2SignedDistPointToPlane_OneDimensional(p1.x, -1.0f, B.min.x);
1524 float da1 = c2SignedDistPointToPlane_OneDimensional(p0.x, 1.0f, B.max.x);
1525 float db1 = c2SignedDistPointToPlane_OneDimensional(p1.x, 1.0f, B.max.x);
1526 float da2 = c2SignedDistPointToPlane_OneDimensional(p0.y, -1.0f, B.min.y);
1527 float db2 = c2SignedDistPointToPlane_OneDimensional(p1.y, -1.0f, B.min.y);
1528 float da3 = c2SignedDistPointToPlane_OneDimensional(p0.y, 1.0f, B.max.y);
1529 float db3 = c2SignedDistPointToPlane_OneDimensional(p1.y, 1.0f, B.max.y);
1530 float t0 = c2RayToPlane_OneDimensional(da0, db0);
1531 float t1 = c2RayToPlane_OneDimensional(da1, db1);
1532 float t2 = c2RayToPlane_OneDimensional(da2, db2);
1533 float t3 = c2RayToPlane_OneDimensional(da3, db3);
1535 // Calculate hit predicate, no branching.
1536 int hit0 = t0 < 1.0f;
1537 int hit1 = t1 < 1.0f;
1538 int hit2 = t2 < 1.0f;
1539 int hit3 = t3 < 1.0f;
1540 int hit = hit0 | hit1 | hit2 | hit3;
1544 // Remap t's within 0-1 range, where >= 1 is treated as 0.
1545 t0 = (float)hit0 * t0;
1546 t1 = (float)hit1 * t1;
1547 t2 = (float)hit2 * t2;
1548 t3 = (float)hit3 * t3;
1550 // Sort output by finding largest t to deduce the normal.
1551 if (t0 >= t1 && t0 >= t2 && t0 >= t3)
1554 out->n = c2V(-1, 0);
1557 else if (t1 >= t0 && t1 >= t2 && t1 >= t3)
1563 else if (t2 >= t0 && t2 >= t1 && t2 >= t3)
1566 out->n = c2V(0, -1);
1576 } else return 0; // This can still numerically happen.
1579 int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out)
1582 M.y = c2Norm(c2Sub(B.b, B.a));
1585 // rotate capsule to origin, along Y axis
1586 // rotate the ray same way
1587 c2v cap_n = c2Sub(B.b, B.a);
1588 c2v yBb = c2MulmvT(M, cap_n);
1589 c2v yAp = c2MulmvT(M, c2Sub(A.p, B.a));
1590 c2v yAd = c2MulmvT(M, A.d);
1591 c2v yAe = c2Add(yAp, c2Mulvs(yAd, A.t));
1594 capsule_bb.min = c2V(-B.r, 0);
1595 capsule_bb.max = c2V(B.r, yBb.y);
1597 out->n = c2Norm(cap_n);
1600 // check and see if ray starts within the capsule
1601 if (c2AABBtoPoint(capsule_bb, yAp)) {
1611 if (c2CircleToPoint(capsule_a, A.p)) {
1613 } else if (c2CircleToPoint(capsule_b, A.p)) {
1618 if (yAe.x * yAp.x < 0 || c2Min(c2Abs(yAe.x), c2Abs(yAp.x)) < B.r)
1626 // ray starts inside capsule prism -- must hit one of the semi-circles
1627 if (c2Abs(yAp.x) < B.r) {
1628 if (yAp.y < 0) return c2RaytoCircle(A, Ca, out);
1629 else return c2RaytoCircle(A, Cb, out);
1632 // hit the capsule prism
1635 float c = yAp.x > 0 ? B.r : -B.r;
1636 float d = (yAe.x - yAp.x);
1637 float t = (c - yAp.x) / d;
1638 float y = yAp.y + (yAe.y - yAp.y) * t;
1639 if (y <= 0) return c2RaytoCircle(A, Ca, out);
1640 if (y >= yBb.y) return c2RaytoCircle(A, Cb, out);
1642 out->n = c > 0 ? M.x : c2Skew(M.y);
1652 int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out)
1654 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
1655 c2v p = c2MulxvT(bx, A.p);
1656 c2v d = c2MulrvT(bx.r, A.d);
1661 // test ray to each plane, tracking lo/hi times of intersection
1662 for (int i = 0; i < B->count; ++i)
1664 float num = c2Dot(B->norms[i], c2Sub(B->verts[i], p));
1665 float den = c2Dot(B->norms[i], d);
1666 if (den == 0 && num < 0) return 0;
1669 if (den < 0 && num < lo * den)
1674 else if (den > 0 && num < hi * den) hi = num / den;
1676 if (hi < lo) return 0;
1682 out->n = c2Mulrv(bx.r, B->norms[index]);
1689 void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m)
1692 c2v d = c2Sub(B.p, A.p);
1693 float d2 = c2Dot(d, d);
1694 float r = A.r + B.r;
1697 float l = c2Sqrt(d2);
1698 c2v n = l != 0 ? c2Mulvs(d, 1.0f / l) : c2V(0, 1.0f);
1700 m->depths[0] = r - l;
1701 m->contact_points[0] = c2Sub(B.p, c2Mulvs(n, B.r));
1706 void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m)
1709 c2v L = c2Clampv(A.p, B.min, B.max);
1710 c2v ab = c2Sub(L, A.p);
1711 float d2 = c2Dot(ab, ab);
1712 float r2 = A.r * A.r;
1715 // shallow (center of circle not inside of AABB)
1718 float d = c2Sqrt(d2);
1721 m->depths[0] = A.r - d;
1722 m->contact_points[0] = c2Add(A.p, c2Mulvs(n, d));
1726 // deep (center of circle inside of AABB)
1727 // clamp circle's center to edge of AABB, then form the manifold
1730 c2v mid = c2Mulvs(c2Add(B.min, B.max), 0.5f);
1731 c2v e = c2Mulvs(c2Sub(B.max, B.min), 0.5f);
1732 c2v d = c2Sub(A.p, mid);
1733 c2v abs_d = c2Absv(d);
1735 float x_overlap = e.x - abs_d.x;
1736 float y_overlap = e.y - abs_d.y;
1741 if (x_overlap < y_overlap)
1745 n = c2Mulvs(n, d.x < 0 ? 1.0f : -1.0f);
1752 n = c2Mulvs(n, d.y < 0 ? 1.0f : -1.0f);
1756 m->depths[0] = A.r + depth;
1757 m->contact_points[0] = c2Sub(A.p, c2Mulvs(n, depth));
1763 void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m)
1767 float r = A.r + B.r;
1768 float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0);
1772 if (d == 0) n = c2Norm(c2Skew(c2Sub(B.b, B.a)));
1773 else n = c2Norm(c2Sub(b, a));
1776 m->depths[0] = r - d;
1777 m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
1782 void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m)
1785 c2v mid_a = c2Mulvs(c2Add(A.min, A.max), 0.5f);
1786 c2v mid_b = c2Mulvs(c2Add(B.min, B.max), 0.5f);
1787 c2v eA = c2Absv(c2Mulvs(c2Sub(A.max, A.min), 0.5f));
1788 c2v eB = c2Absv(c2Mulvs(c2Sub(B.max, B.min), 0.5f));
1789 c2v d = c2Sub(mid_b, mid_a);
1791 // calc overlap on x and y axes
1792 float dx = eA.x + eB.x - c2Abs(d.x);
1794 float dy = eA.y + eB.y - c2Abs(d.y);
1801 // x axis overlap is smaller
1808 p = c2Sub(mid_a, c2V(eA.x, 0));
1813 p = c2Add(mid_a, c2V(eA.x, 0));
1817 // y axis overlap is smaller
1824 p = c2Sub(mid_a, c2V(0, eA.y));
1829 p = c2Add(mid_a, c2V(0, eA.y));
1834 m->contact_points[0] = p;
1835 m->depths[0] = depth;
1839 void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m)
1843 c2BBVerts(p.verts, &A);
1845 c2Norms(p.verts, p.norms, 4);
1846 c2CapsuletoPolyManifold(B, &p, 0, m);
1850 void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m)
1854 float r = A.r + B.r;
1855 float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0);
1859 if (d == 0) n = c2Norm(c2Skew(c2Sub(A.b, A.a)));
1860 else n = c2Norm(c2Sub(b, a));
1863 m->depths[0] = r - d;
1864 m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
1869 static C2_INLINE c2h c2PlaneAt(const c2Poly* p, const int i)
1873 h.d = c2Dot(p->norms[i], p->verts[i]);
1877 void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx_tr, c2Manifold* m)
1881 float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx_tr, &a, &b, 0, 0, 0);
1883 // shallow, the circle center did not hit the polygon
1884 // just use a and b from GJK to define the collision
1887 c2v n = c2Sub(b, a);
1888 float l = c2Dot(n, n);
1893 m->contact_points[0] = b;
1894 m->depths[0] = A.r - l;
1895 m->n = c2Mulvs(n, 1.0f / l);
1899 // Circle center is inside the polygon
1900 // find the face closest to circle center to form manifold
1903 c2x bx = bx_tr ? *bx_tr : c2xIdentity();
1904 float sep = -FLT_MAX;
1906 c2v local = c2MulxvT(bx, A.p);
1908 for (int i = 0; i < B->count; ++i)
1910 c2h h = c2PlaneAt(B, i);
1911 d = c2Dist(h, local);
1912 if (d > A.r) return;
1920 c2h h = c2PlaneAt(B, index);
1921 c2v p = c2Project(h, local);
1923 m->contact_points[0] = c2Mulxv(bx, p);
1924 m->depths[0] = A.r - sep;
1925 m->n = c2Neg(c2Mulrv(bx.r, B->norms[index]));
1929 // Forms a c2Poly and uses c2PolytoPolyManifold
1930 void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m)
1934 c2BBVerts(p.verts, &A);
1936 c2Norms(p.verts, p.norms, 4);
1937 c2PolytoPolyManifold(&p, 0, B, bx, m);
1940 // clip a segment to a plane
1941 static int c2Clip(c2v* seg, c2h h)
1946 if ((d0 = c2Dist(h, seg[0])) < 0) out[sp++] = seg[0];
1947 if ((d1 = c2Dist(h, seg[1])) < 0) out[sp++] = seg[1];
1948 if (d0 == 0 && d1 == 0) {
1951 } else if (d0 * d1 <= 0) out[sp++] = c2Intersect(seg[0], seg[1], d0, d1);
1952 seg[0] = out[0]; seg[1] = out[1];
1957 #pragma warning(push)
1958 #pragma warning(disable:4204) // nonstandard extension used: non-constant aggregate initializer
1961 static int c2SidePlanes(c2v* seg, c2v ra, c2v rb, c2h* h)
1963 c2v in = c2Norm(c2Sub(rb, ra));
1964 c2h left = { c2Neg(in), c2Dot(c2Neg(in), ra) };
1965 c2h right = { in, c2Dot(in, rb) };
1966 if (c2Clip(seg, left) < 2) return 0;
1967 if (c2Clip(seg, right) < 2) return 0;
1970 h->d = c2Dot(c2CCW90(in), ra);
1975 // clip a segment to the "side planes" of another segment.
1976 // side planes are planes orthogonal to a segment and attached to the
1977 // endpoints of the segment
1978 static int c2SidePlanesFromPoly(c2v* seg, c2x x, const c2Poly* p, int e, c2h* h)
1980 c2v ra = c2Mulxv(x, p->verts[e]);
1981 c2v rb = c2Mulxv(x, p->verts[e + 1 == p->count ? 0 : e + 1]);
1982 return c2SidePlanes(seg, ra, rb, h);
1985 static void c2KeepDeep(c2v* seg, c2h h, c2Manifold* m)
1988 for (int i = 0; i < 2; ++i)
1991 float d = c2Dist(h, p);
1994 m->contact_points[cp] = p;
2003 static C2_INLINE c2v c2CapsuleSupport(c2Capsule A, c2v dir)
2005 float da = c2Dot(A.a, dir);
2006 float db = c2Dot(A.b, dir);
2007 if (da > db) return c2Add(A.a, c2Mulvs(dir, A.r));
2008 else return c2Add(A.b, c2Mulvs(dir, A.r));
2011 static void c2AntinormalFace(c2Capsule cap, const c2Poly* p, c2x x, int* face_out, c2v* n_out)
2013 float sep = -FLT_MAX;
2016 for (int i = 0; i < p->count; ++i)
2018 c2h h = c2Mulxh(x, c2PlaneAt(p, i));
2019 c2v n0 = c2Neg(h.n);
2020 c2v s = c2CapsuleSupport(cap, n0);
2021 float d = c2Dist(h, s);
2033 static void c2Incident(c2v* incident, const c2Poly* ip, c2x ix, c2v rn_in_incident_space)
2036 float min_dot = FLT_MAX;
2037 for (int i = 0; i < ip->count; ++i)
2039 float dot = c2Dot(rn_in_incident_space, ip->norms[i]);
2046 incident[0] = c2Mulxv(ix, ip->verts[index]);
2047 incident[1] = c2Mulxv(ix, ip->verts[index + 1 == ip->count ? 0 : index + 1]);
2050 void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m)
2054 float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx_ptr, &a, &b, 0, 0, 0);
2056 // deep, treat as segment to poly collision
2059 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
2061 A_in_B.a = c2MulxvT(bx, A.a);
2062 A_in_B.b = c2MulxvT(bx, A.b);
2063 c2v ab = c2Norm(c2Sub(A_in_B.a, A_in_B.b));
2065 // test capsule axes
2067 ab_h0.n = c2CCW90(ab);
2068 ab_h0.d = c2Dot(A_in_B.a, ab_h0.n);
2069 int v0 = c2Support(B->verts, B->count, c2Neg(ab_h0.n));
2070 float s0 = c2Dist(ab_h0, B->verts[v0]);
2073 ab_h1.n = c2Skew(ab);
2074 ab_h1.d = c2Dot(A_in_B.a, ab_h1.n);
2075 int v1 = c2Support(B->verts, B->count, c2Neg(ab_h1.n));
2076 float s1 = c2Dist(ab_h1, B->verts[v1]);
2080 float sep = -FLT_MAX;
2082 for (int i = 0; i < B->count; ++i)
2084 c2h h = c2PlaneAt(B, i);
2085 float da = c2Dot(A_in_B.a, c2Neg(h.n));
2086 float db = c2Dot(A_in_B.b, c2Neg(h.n));
2088 if (da > db) d = c2Dist(h, A_in_B.a);
2089 else d = c2Dist(h, A_in_B.b);
2097 // track axis of minimum separation
2112 case 0: // poly face
2114 c2v seg[2] = { A.a, A.b };
2116 if (!c2SidePlanesFromPoly(seg, bx, B, index, &h)) return;
2117 c2KeepDeep(seg, h, m);
2121 case 1: // side 0 of capsule segment
2124 c2Incident(incident, B, bx, ab_h0.n);
2126 if (!c2SidePlanes(incident, A_in_B.b, A_in_B.a, &h)) return;
2127 c2KeepDeep(incident, h, m);
2130 case 2: // side 1 of capsule segment
2133 c2Incident(incident, B, bx, ab_h1.n);
2135 if (!c2SidePlanes(incident, A_in_B.a, A_in_B.b, &h)) return;
2136 c2KeepDeep(incident, h, m);
2140 // should never happen.
2144 for (int i = 0; i < m->count; ++i) m->depths[i] += A.r;
2147 // shallow, use GJK results a and b to define manifold
2151 m->n = c2Norm(c2Sub(b, a));
2152 m->contact_points[0] = c2Add(a, c2Mulvs(m->n, A.r));
2153 m->depths[0] = A.r - d;
2158 #pragma warning(pop)
2161 static float c2CheckFaces(const c2Poly* A, c2x ax, const c2Poly* B, c2x bx, int* face_index)
2163 c2x b_in_a = c2MulxxT(ax, bx);
2164 c2x a_in_b = c2MulxxT(bx, ax);
2165 float sep = -FLT_MAX;
2168 for (int i = 0; i < A->count; ++i)
2170 c2h h = c2PlaneAt(A, i);
2171 int idx = c2Support(B->verts, B->count, c2Mulrv(a_in_b.r, c2Neg(h.n)));
2172 c2v p = c2Mulxv(b_in_a, B->verts[idx]);
2173 float d = c2Dist(h, p);
2181 *face_index = index;
2185 // Please see Dirk Gregorius's 2013 GDC lecture on the Separating Axis Theorem
2186 // for a full-algorithm overview. The short description is:
2187 // Test A against faces of B, test B against faces of A
2188 // Define the reference and incident shapes (denoted by r and i respectively)
2189 // Define the reference face as the axis of minimum penetration
2190 // Find the incident face, which is most anti-normal face
2191 // Clip incident face to reference face side planes
2192 // Keep all points behind the reference face
2193 void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax_ptr, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m)
2196 c2x ax = ax_ptr ? *ax_ptr : c2xIdentity();
2197 c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
2200 if ((sa = c2CheckFaces(A, ax, B, bx, &ea)) >= 0) return;
2201 if ((sb = c2CheckFaces(B, bx, A, ax, &eb)) >= 0) return;
2203 const c2Poly* rp,* ip;
2206 float kRelTol = 0.95f, kAbsTol = 0.01f;
2208 if (sa * kRelTol > sb + kAbsTol)
2224 c2Incident(incident, ip, ix, c2MulrvT(ix.r, c2Mulrv(rx.r, rp->norms[re])));
2226 if (!c2SidePlanesFromPoly(incident, rx, rp, re, &rh)) return;
2227 c2KeepDeep(incident, rh, m);
2228 if (flip) m->n = c2Neg(m->n);
2231 #endif // CUTE_C2_IMPLEMENTATION_ONCE
2232 #endif // CUTE_C2_IMPLEMENTATION