From 0106e2673f623f383c57cee82f5b8309973e289e Mon Sep 17 00:00:00 2001 From: En Yi Date: Sat, 4 Jan 2025 14:36:07 +0800 Subject: [PATCH] Integrate cute_c2 collision + extension --- engine/geometry/CMakeLists.txt | 9 + engine/geometry/cute_c2.h | 2274 +++++++++++++++++ engine/geometry/cute_c2_ext.h | 35 + engine/geometry/geometry.c | 643 +++++ engine/tests/CMakeLists.txt | 1 + engine/tests/geometry/CMakeLists.txt | 1 + engine/tests/geometry/samples/CMakeLists.txt | 8 + .../tests/geometry/samples/collision_sample.c | 209 ++ 8 files changed, 3180 insertions(+) create mode 100644 engine/geometry/CMakeLists.txt create mode 100644 engine/geometry/cute_c2.h create mode 100644 engine/geometry/cute_c2_ext.h create mode 100644 engine/geometry/geometry.c create mode 100644 engine/tests/geometry/CMakeLists.txt create mode 100644 engine/tests/geometry/samples/CMakeLists.txt create mode 100644 engine/tests/geometry/samples/collision_sample.c diff --git a/engine/geometry/CMakeLists.txt b/engine/geometry/CMakeLists.txt new file mode 100644 index 0000000..9315899 --- /dev/null +++ b/engine/geometry/CMakeLists.txt @@ -0,0 +1,9 @@ +add_library(geometry + STATIC + geometry.c +) + +target_include_directories(geometry + PUBLIC + ${CMAKE_CURRENT_LIST_DIR} +) diff --git a/engine/geometry/cute_c2.h b/engine/geometry/cute_c2.h new file mode 100644 index 0000000..a5f87bd --- /dev/null +++ b/engine/geometry/cute_c2.h @@ -0,0 +1,2274 @@ +/* + ------------------------------------------------------------------------------ + Licensing information can be found at the end of the file. + ------------------------------------------------------------------------------ + + cute_c2.h - v1.10 + + To create implementation (the function definitions) + #define CUTE_C2_IMPLEMENTATION + in *one* C/CPP file (translation unit) that includes this file + + + SUMMARY + + cute_c2 is a single-file header that implements 2D collision detection routines + that test for overlap, and optionally can find the collision manifold. The + manifold contains all necessary information to prevent shapes from inter- + penetrating, which is useful for character controllers, general physics + simulation, and user-interface programming. + + This header implements a group of "immediate mode" functions that should be + very easily adapted into pre-existing projects. + + + THE IMPORTANT PARTS + + Most of the math types in this header are for internal use. Users care about + the shape types and the collision functions. + + SHAPE TYPES: + * c2Circle + * c2Capsule + * c2AABB + * c2Ray + * c2Poly + + COLLISION FUNCTIONS (*** is a shape name from the above list): + * c2***to*** - boolean YES/NO hittest + * c2***to***Manifold - construct manifold to describe how shapes hit + * c2GJK - runs GJK algorithm to find closest point pair between two shapes + * c2TOI - computes the time of impact between two shapes, useful for sweeping shapes, or doing shape casts + * c2MakePoly - Runs convex hull algorithm and computes normals on input point-set + * c2Collided - generic version of c2***to*** funcs + * c2Collide - generic version of c2***to***Manifold funcs + * c2CastRay - generic version of c2Rayto*** funcs + + The rest of the header is more or less for internal use. Here is an example of + making some shapes and testing for collision: + + c2Circle c; + c.p = position; + c.r = radius; + + c2Capsule cap; + cap.a = first_endpoint; + cap.b = second_endpoint; + cap.r = radius; + + int hit = c2CircletoCapsule(c, cap); + if (hit) + { + handle collision here... + } + + For more code examples and tests please see: + https://github.com/RandyGaul/cute_header/tree/master/examples_cute_gl_and_c2 + + Here is a past discussion thread on this header: + https://www.reddit.com/r/gamedev/comments/5tqyey/tinyc2_2d_collision_detection_library_in_c/ + + Here is a very nice repo containing various tests and examples using SFML for rendering: + https://github.com/sro5h/tinyc2-tests + + + FEATURES + + * Circles, capsules, AABBs, rays and convex polygons are supported + * Fast boolean only result functions (hit yes/no) + * Slghtly slower manifold generation for collision normals + depths +points + * GJK implementation (finds closest points for disjoint pairs of shapes) + * Shape casts/sweeps with c2TOI function (time of impact) + * Robust 2D convex hull generator + * Lots of correctly implemented and tested 2D math routines + * Implemented in portable C, and is readily portable to other languages + * Generic c2Collide, c2Collided and c2CastRay function (can pass in any shape type) + * Extensive examples at: https://github.com/RandyGaul/cute_headers/tree/master/examples_cute_gl_and_c2 + + + Revision History + + 1.0 (02/13/2017) initial release + 1.01 (02/13/2017) const crusade, minor optimizations, capsule degen + 1.02 (03/21/2017) compile fixes for c on more compilers + 1.03 (09/15/2017) various bugfixes and quality of life changes to manifolds + 1.04 (03/25/2018) fixed manifold bug in c2CircletoAABBManifold + 1.05 (11/01/2018) added c2TOI (time of impact) for shape cast/sweep test + 1.06 (08/23/2019) C2_*** types to C2_TYPE_***, and CUTE_C2_API + 1.07 (10/19/2019) Optimizations to c2TOI - breaking change to c2GJK API + 1.08 (12/22/2019) Remove contact point + normal from c2TOI, removed feather + radius from c2GJK, fixed various bugs in capsule to poly + manifold, did a pass on all docs + 1.09 (07/27/2019) Added c2Inflate - to inflate/deflate shapes for c2TOI + 1.10 (02/05/2022) Implemented GJK-Raycast for c2TOI (from E. Catto's Box2D) + + + Contributors + + Plastburk 1.01 - const pointers pull request + mmozeiko 1.02 - 3 compile bugfixes + felipefs 1.02 - 3 compile bugfixes + seemk 1.02 - fix branching bug in c2Collide + sro5h 1.02 - bug reports for multiple manifold funcs + sro5h 1.03 - work involving quality of life fixes for manifolds + Wizzard033 1.06 - C2_*** types to C2_TYPE_***, and CUTE_C2_API + Tyler Glaeil 1.08 - Lots of bug reports and disussion on capsules + TOIs + + + DETAILS/ADVICE + + BROAD PHASE + + This header does not implement a broad-phase, and instead concerns itself with + the narrow-phase. This means this header just checks to see if two individual + shapes are touching, and can give information about how they are touching. + + Very common 2D broad-phases are tree and grid approaches. Quad trees are good + for static geometry that does not move much if at all. Dynamic AABB trees are + good for general purpose use, and can handle moving objects very well. Grids + are great and are similar to quad trees. + + If implementing a grid it can be wise to have each collideable grid cell hold + an integer. This integer refers to a 2D shape that can be passed into the + various functions in this header. The shape can be transformed from "model" + space to "world" space using c2x -- a transform struct. In this way a grid + can be implemented that holds any kind of convex shape (that this header + supports) while conserving memory with shape instancing. + + NUMERIC ROBUSTNESS + + Many of the functions in cute c2 use `c2GJK`, an implementation of the GJK + algorithm. Internally GJK computes signed area values, and these values are + very numerically sensitive to large shapes. This means the GJK function will + break down if input shapes are too large or too far away from the origin. + + In general it is best to compute collision detection on small shapes very + close to the origin. One trick is to keep your collision information numerically + very tiny, and simply scale it up when rendering to the appropriate size. + + For reference, if your shapes are all AABBs and contain a width and height + of somewhere between 1.0f and 10.0f, everything will be fine. However, once + your shapes start approaching a width/height of 100.0f to 1000.0f GJK can + start breaking down. + + This is a complicated topic, so feel free to ask the author for advice here. + + Here is an example demonstrating this problem with two large AABBs: + https://github.com/RandyGaul/cute_headers/issues/160 + + Please email at my address with any questions or comments at: + author's last name followed by 1748 at gmail +*/ + +#if !defined(CUTE_C2_H) + +// this can be adjusted as necessary, but is highly recommended to be kept at 8. +// higher numbers will incur quite a bit of memory overhead, and convex shapes +// over 8 verts start to just look like spheres, which can be implicitly rep- +// resented as a point + radius. usually tools that generate polygons should be +// constructed so they do not output polygons with too many verts. +// Note: polygons in cute_c2 are all *convex*. +#define C2_MAX_POLYGON_VERTS 8 + +// 2d vector +typedef struct c2v +{ + float x; + float y; +} c2v; + +// 2d rotation composed of cos/sin pair for a single angle +// We use two floats as a small optimization to avoid computing sin/cos unnecessarily +typedef struct c2r +{ + float c; + float s; +} c2r; + +// 2d rotation matrix +typedef struct c2m +{ + c2v x; + c2v y; +} c2m; + +// 2d transformation "x" +// These are used especially for c2Poly when a c2Poly is passed to a function. +// Since polygons are prime for "instancing" a c2x transform can be used to +// transform a polygon from local space to world space. In functions that take +// a c2x pointer (like c2PolytoPoly), these pointers can be NULL, which represents +// an identity transformation and assumes the verts inside of c2Poly are already +// in world space. +typedef struct c2x +{ + c2v p; + c2r r; +} c2x; + +// 2d halfspace (aka plane, aka line) +typedef struct c2h +{ + c2v n; // normal, normalized + float d; // distance to origin from plane, or ax + by = d +} c2h; + +typedef struct c2Circle +{ + c2v p; + float r; +} c2Circle; + +typedef struct c2AABB +{ + c2v min; + c2v max; +} c2AABB; + +// a capsule is defined as a line segment (from a to b) and radius r +typedef struct c2Capsule +{ + c2v a; + c2v b; + float r; +} c2Capsule; + +typedef struct c2Poly +{ + int count; + c2v verts[C2_MAX_POLYGON_VERTS]; + c2v norms[C2_MAX_POLYGON_VERTS]; +} c2Poly; + +// IMPORTANT: +// Many algorithms in this file are sensitive to the magnitude of the +// ray direction (c2Ray::d). It is highly recommended to normalize the +// ray direction and use t to specify a distance. Please see this link +// for an in-depth explanation: https://github.com/RandyGaul/cute_headers/issues/30 +typedef struct c2Ray +{ + c2v p; // position + c2v d; // direction (normalized) + float t; // distance along d from position p to find endpoint of ray +} c2Ray; + +typedef struct c2Raycast +{ + float t; // time of impact + c2v n; // normal of surface at impact (unit length) +} c2Raycast; + +// position of impact p = ray.p + ray.d * raycast.t +#define c2Impact(ray, t) c2Add(ray.p, c2Mulvs(ray.d, t)) + +// contains all information necessary to resolve a collision, or in other words +// this is the information needed to separate shapes that are colliding. Doing +// the resolution step is *not* included in cute_c2. +typedef struct c2Manifold +{ + int count; + float depths[2]; + c2v contact_points[2]; + + // always points from shape A to shape B (first and second shapes passed into + // any of the c2***to***Manifold functions) + c2v n; +} c2Manifold; + +// This define allows exporting/importing of the header to a dynamic library. +// Here's an example. +// #define CUTE_C2_API extern "C" __declspec(dllexport) +#if !defined(CUTE_C2_API) +# define CUTE_C2_API +#endif + +// boolean collision detection +// these versions are faster than the manifold versions, but only give a YES/NO result +CUTE_C2_API int c2CircletoCircle(c2Circle A, c2Circle B); +CUTE_C2_API int c2CircletoAABB(c2Circle A, c2AABB B); +CUTE_C2_API int c2CircletoCapsule(c2Circle A, c2Capsule B); +CUTE_C2_API int c2AABBtoAABB(c2AABB A, c2AABB B); +CUTE_C2_API int c2AABBtoCapsule(c2AABB A, c2Capsule B); +CUTE_C2_API int c2CapsuletoCapsule(c2Capsule A, c2Capsule B); +CUTE_C2_API int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx); +CUTE_C2_API int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx); +CUTE_C2_API int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx); +CUTE_C2_API int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx); + +// ray operations +// output is placed into the c2Raycast struct, which represents the hit location +// of the ray. the out param contains no meaningful information if these funcs +// return 0 +CUTE_C2_API int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out); +CUTE_C2_API int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out); +CUTE_C2_API int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out); +CUTE_C2_API int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out); + +// manifold generation +// These functions are (generally) slower than the boolean versions, but will compute one +// or two points that represent the plane of contact. This information is usually needed +// to resolve and prevent shapes from colliding. If no collision occured the count member +// of the manifold struct is set to 0. +CUTE_C2_API void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m); +CUTE_C2_API void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m); +CUTE_C2_API void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m); +CUTE_C2_API void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m); +CUTE_C2_API void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m); +CUTE_C2_API void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m); +CUTE_C2_API void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx, c2Manifold* m); +CUTE_C2_API void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m); +CUTE_C2_API void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx, c2Manifold* m); +CUTE_C2_API void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx, c2Manifold* m); + +typedef enum +{ + C2_TYPE_NONE, + C2_TYPE_CIRCLE, + C2_TYPE_AABB, + C2_TYPE_CAPSULE, + C2_TYPE_POLY +} C2_TYPE; + +// This struct is only for advanced usage of the c2GJK function. See comments inside of the +// c2GJK function for more details. +typedef struct c2GJKCache +{ + float metric; + int count; + int iA[3]; + int iB[3]; + float div; +} c2GJKCache; + +// This is an advanced function, intended to be used by people who know what they're doing. +// +// Runs the GJK algorithm to find closest points, returns distance between closest points. +// outA and outB can be NULL, in this case only distance is returned. ax_ptr and bx_ptr +// can be NULL, and represent local to world transformations for shapes A and B respectively. +// use_radius will apply radii for capsules and circles (if set to false, spheres are +// treated as points and capsules are treated as line segments i.e. rays). The cache parameter +// should be NULL, as it is only for advanced usage (unless you know what you're doing, then +// go ahead and use it). iterations is an optional parameter. +// +// IMPORTANT NOTE: +// The GJK function is sensitive to large shapes, since it internally will compute signed area +// values. `c2GJK` is called throughout cute c2 in many ways, so try to make sure all of your +// collision shapes are not gigantic. For example, try to keep the volume of all your shapes +// less than 100.0f. If you need large shapes, you should use tiny collision geometry for all +// cute c2 function, and simply render the geometry larger on-screen by scaling it up. +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); + +// Stores results of a time of impact calculation done by `c2TOI`. +typedef struct c2TOIResult +{ + int hit; // 1 if shapes were touching at the TOI, 0 if they never hit. + float toi; // The time of impact between two shapes. + c2v n; // Surface normal from shape A to B at the time of impact. + c2v p; // Point of contact between shapes A and B at time of impact. + int iterations; // Number of iterations the solver underwent. +} c2TOIResult; + +// This is an advanced function, intended to be used by people who know what they're doing. +// +// Computes the time of impact from shape A and shape B. The velocity of each shape is provided +// by vA and vB respectively. The shapes are *not* allowed to rotate over time. The velocity is +// assumed to represent the change in motion from time 0 to time 1, and so the return value will +// be a number from 0 to 1. To move each shape to the colliding configuration, multiply vA and vB +// each by the return value. ax_ptr and bx_ptr are optional parameters to transforms for each shape, +// and are typically used for polygon shapes to transform from model to world space. Set these to +// NULL to represent identity transforms. iterations is an optional parameter. use_radius +// will apply radii for capsules and circles (if set to false, spheres are treated as points and +// capsules are treated as line segments i.e. rays). +// +// IMPORTANT NOTE: +// The c2TOI function can be used to implement a "swept character controller", but it can be +// difficult to do so. Say we compute a time of impact with `c2TOI` and move the shapes to the +// time of impact, and adjust the velocity by zeroing out the velocity along the surface normal. +// If we then call `c2TOI` again, it will fail since the shapes will be considered to start in +// a colliding configuration. There are many styles of tricks to get around this problem, and +// all of them involve giving the next call to `c2TOI` some breathing room. It is recommended +// to use some variation of the following algorithm: +// +// 1. Call c2TOI. +// 2. Move the shapes to the TOI. +// 3. Slightly inflate the size of one, or both, of the shapes so they will be intersecting. +// The purpose is to make the shapes numerically intersecting, but not visually intersecting. +// Another option is to call c2TOI with slightly deflated shapes. +// See the function `c2Inflate` for some more details. +// 4. Compute the collision manifold between the inflated shapes (for example, use c2PolytoPolyManifold). +// 5. Gently push the shapes apart. This will give the next call to c2TOI some breathing room. +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); + +// Inflating a shape. +// +// This is useful to numerically grow or shrink a polytope. For example, when calling +// a time of impact function it can be good to use a slightly smaller shape. Then, once +// both shapes are moved to the time of impact a collision manifold can be made from the +// slightly larger (and now overlapping) shapes. +// +// IMPORTANT NOTE +// Inflating a shape with sharp corners can cause those corners to move dramatically. +// Deflating a shape can avoid this problem, but deflating a very small shape can invert +// the planes and result in something that is no longer convex. Make sure to pick an +// appropriately small skin factor, for example 1.0e-6f. +CUTE_C2_API void c2Inflate(void* shape, C2_TYPE type, float skin_factor); + +// Computes 2D convex hull. Will not do anything if less than two verts supplied. If +// more than C2_MAX_POLYGON_VERTS are supplied extras are ignored. +CUTE_C2_API int c2Hull(c2v* verts, int count); +CUTE_C2_API void c2Norms(c2v* verts, c2v* norms, int count); + +// runs c2Hull and c2Norms, assumes p->verts and p->count are both set to valid values +CUTE_C2_API void c2MakePoly(c2Poly* p); + +// Generic collision detection routines, useful for games that want to use some poly- +// morphism to write more generic-styled code. Internally calls various above functions. +// For AABBs/Circles/Capsules ax and bx are ignored. For polys ax and bx can define +// model to world transformations (for polys only), or be NULL for identity transforms. +CUTE_C2_API int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB); +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); +CUTE_C2_API int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out); + +#ifdef _MSC_VER + #define C2_INLINE __forceinline +#else + #define C2_INLINE inline __attribute__((always_inline)) +#endif + +// adjust these primitives as seen fit +#include // memcpy +#include +#define c2Sin(radians) sinf(radians) +#define c2Cos(radians) cosf(radians) +#define c2Sqrt(a) sqrtf(a) +#define c2Min(a, b) ((a) < (b) ? (a) : (b)) +#define c2Max(a, b) ((a) > (b) ? (a) : (b)) +#define c2Abs(a) ((a) < 0 ? -(a) : (a)) +#define c2Clamp(a, lo, hi) c2Max(lo, c2Min(a, hi)) +C2_INLINE void c2SinCos(float radians, float* s, float* c) { *c = c2Cos(radians); *s = c2Sin(radians); } +#define c2Sign(a) (a < 0 ? -1.0f : 1.0f) + +// The rest of the functions in the header-only portion are all for internal use +// and use the author's personal naming conventions. It is recommended to use one's +// own math library instead of the one embedded here in cute_c2, but for those +// curious or interested in trying it out here's the details: + +// The Mul functions are used to perform multiplication. x stands for transform, +// v stands for vector, s stands for scalar, r stands for rotation, h stands for +// halfspace and T stands for transpose.For example c2MulxvT stands for "multiply +// a transform with a vector, and transpose the transform". + +// vector ops +C2_INLINE c2v c2V(float x, float y) { c2v a; a.x = x; a.y = y; return a; } +C2_INLINE c2v c2Add(c2v a, c2v b) { a.x += b.x; a.y += b.y; return a; } +C2_INLINE c2v c2Sub(c2v a, c2v b) { a.x -= b.x; a.y -= b.y; return a; } +C2_INLINE float c2Dot(c2v a, c2v b) { return a.x * b.x + a.y * b.y; } +C2_INLINE c2v c2Mulvs(c2v a, float b) { a.x *= b; a.y *= b; return a; } +C2_INLINE c2v c2Mulvv(c2v a, c2v b) { a.x *= b.x; a.y *= b.y; return a; } +C2_INLINE c2v c2Div(c2v a, float b) { return c2Mulvs(a, 1.0f / b); } +C2_INLINE c2v c2Skew(c2v a) { c2v b; b.x = -a.y; b.y = a.x; return b; } +C2_INLINE c2v c2CCW90(c2v a) { c2v b; b.x = a.y; b.y = -a.x; return b; } +C2_INLINE float c2Det2(c2v a, c2v b) { return a.x * b.y - a.y * b.x; } +C2_INLINE c2v c2Minv(c2v a, c2v b) { return c2V(c2Min(a.x, b.x), c2Min(a.y, b.y)); } +C2_INLINE c2v c2Maxv(c2v a, c2v b) { return c2V(c2Max(a.x, b.x), c2Max(a.y, b.y)); } +C2_INLINE c2v c2Clampv(c2v a, c2v lo, c2v hi) { return c2Maxv(lo, c2Minv(a, hi)); } +C2_INLINE c2v c2Absv(c2v a) { return c2V(c2Abs(a.x), c2Abs(a.y)); } +C2_INLINE float c2Hmin(c2v a) { return c2Min(a.x, a.y); } +C2_INLINE float c2Hmax(c2v a) { return c2Max(a.x, a.y); } +C2_INLINE float c2Len(c2v a) { return c2Sqrt(c2Dot(a, a)); } +C2_INLINE c2v c2Norm(c2v a) { return c2Div(a, c2Len(a)); } +C2_INLINE c2v c2SafeNorm(c2v a) { float sq = c2Dot(a, a); return sq ? c2Div(a, c2Len(a)) : c2V(0, 0); } +C2_INLINE c2v c2Neg(c2v a) { return c2V(-a.x, -a.y); } +C2_INLINE c2v c2Lerp(c2v a, c2v b, float t) { return c2Add(a, c2Mulvs(c2Sub(b, a), t)); } +C2_INLINE int c2Parallel(c2v a, c2v b, float kTol) +{ + float k = c2Len(a) / c2Len(b); + b = c2Mulvs(b, k); + if (c2Abs(a.x - b.x) < kTol && c2Abs(a.y - b.y) < kTol) return 1; + return 0; +} + +// rotation ops +C2_INLINE c2r c2Rot(float radians) { c2r r; c2SinCos(radians, &r.s, &r.c); return r; } +C2_INLINE c2r c2RotIdentity(void) { c2r r; r.c = 1.0f; r.s = 0; return r; } +C2_INLINE c2v c2RotX(c2r r) { return c2V(r.c, r.s); } +C2_INLINE c2v c2RotY(c2r r) { return c2V(-r.s, r.c); } +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); } +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); } +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; } +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; } + +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; } +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; } +C2_INLINE c2m c2Mulmm(c2m a, c2m b) { c2m c; c.x = c2Mulmv(a, b.x); c.y = c2Mulmv(a, b.y); return c; } +C2_INLINE c2m c2MulmmT(c2m a, c2m b) { c2m c; c.x = c2MulmvT(a, b.x); c.y = c2MulmvT(a, b.y); return c; } + +// transform ops +C2_INLINE c2x c2xIdentity(void) { c2x x; x.p = c2V(0, 0); x.r = c2RotIdentity(); return x; } +C2_INLINE c2v c2Mulxv(c2x a, c2v b) { return c2Add(c2Mulrv(a.r, b), a.p); } +C2_INLINE c2v c2MulxvT(c2x a, c2v b) { return c2MulrvT(a.r, c2Sub(b, a.p)); } +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; } +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; } +C2_INLINE c2x c2Transform(c2v p, float radians) { c2x x; x.r = c2Rot(radians); x.p = p; return x; } + +// halfspace ops +C2_INLINE c2v c2Origin(c2h h) { return c2Mulvs(h.n, h.d); } +C2_INLINE float c2Dist(c2h h, c2v p) { return c2Dot(h.n, p) - h.d; } +C2_INLINE c2v c2Project(c2h h, c2v p) { return c2Sub(p, c2Mulvs(h.n, c2Dist(h, p))); } +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; } +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; } +C2_INLINE c2v c2Intersect(c2v a, c2v b, float da, float db) { return c2Add(a, c2Mulvs(c2Sub(b, a), (da / (da - db)))); } + +C2_INLINE void c2BBVerts(c2v* out, c2AABB* bb) +{ + out[0] = bb->min; + out[1] = c2V(bb->max.x, bb->min.y); + out[2] = bb->max; + out[3] = c2V(bb->min.x, bb->max.y); +} + +#define CUTE_C2_H +#endif + +#ifdef CUTE_C2_IMPLEMENTATION +#ifndef CUTE_C2_IMPLEMENTATION_ONCE +#define CUTE_C2_IMPLEMENTATION_ONCE + +int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB) +{ + switch (typeA) + { + case C2_TYPE_CIRCLE: + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2CircletoCircle(*(c2Circle*)A, *(c2Circle*)B); + case C2_TYPE_AABB: return c2CircletoAABB(*(c2Circle*)A, *(c2AABB*)B); + case C2_TYPE_CAPSULE: return c2CircletoCapsule(*(c2Circle*)A, *(c2Capsule*)B); + case C2_TYPE_POLY: return c2CircletoPoly(*(c2Circle*)A, (const c2Poly*)B, bx); + default: return 0; + } + break; + + case C2_TYPE_AABB: + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2CircletoAABB(*(c2Circle*)B, *(c2AABB*)A); + case C2_TYPE_AABB: return c2AABBtoAABB(*(c2AABB*)A, *(c2AABB*)B); + case C2_TYPE_CAPSULE: return c2AABBtoCapsule(*(c2AABB*)A, *(c2Capsule*)B); + case C2_TYPE_POLY: return c2AABBtoPoly(*(c2AABB*)A, (const c2Poly*)B, bx); + default: return 0; + } + break; + + case C2_TYPE_CAPSULE: + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2CircletoCapsule(*(c2Circle*)B, *(c2Capsule*)A); + case C2_TYPE_AABB: return c2AABBtoCapsule(*(c2AABB*)B, *(c2Capsule*)A); + case C2_TYPE_CAPSULE: return c2CapsuletoCapsule(*(c2Capsule*)A, *(c2Capsule*)B); + case C2_TYPE_POLY: return c2CapsuletoPoly(*(c2Capsule*)A, (const c2Poly*)B, bx); + default: return 0; + } + break; + + case C2_TYPE_POLY: + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2CircletoPoly(*(c2Circle*)B, (const c2Poly*)A, ax); + case C2_TYPE_AABB: return c2AABBtoPoly(*(c2AABB*)B, (const c2Poly*)A, ax); + case C2_TYPE_CAPSULE: return c2CapsuletoPoly(*(c2Capsule*)B, (const c2Poly*)A, ax); + case C2_TYPE_POLY: return c2PolytoPoly((const c2Poly*)A, ax, (const c2Poly*)B, bx); + default: return 0; + } + break; + + default: + return 0; + } +} + +void c2Collide(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB, c2Manifold* m) +{ + m->count = 0; + + switch (typeA) + { + case C2_TYPE_CIRCLE: + switch (typeB) + { + case C2_TYPE_CIRCLE: c2CircletoCircleManifold(*(c2Circle*)A, *(c2Circle*)B, m); break; + case C2_TYPE_AABB: c2CircletoAABBManifold(*(c2Circle*)A, *(c2AABB*)B, m); break; + case C2_TYPE_CAPSULE: c2CircletoCapsuleManifold(*(c2Circle*)A, *(c2Capsule*)B, m); break; + case C2_TYPE_POLY: c2CircletoPolyManifold(*(c2Circle*)A, (const c2Poly*)B, bx, m); break; + } + break; + + case C2_TYPE_AABB: + switch (typeB) + { + case C2_TYPE_CIRCLE: c2CircletoAABBManifold(*(c2Circle*)B, *(c2AABB*)A, m); m->n = c2Neg(m->n); break; + case C2_TYPE_AABB: c2AABBtoAABBManifold(*(c2AABB*)A, *(c2AABB*)B, m); break; + case C2_TYPE_CAPSULE: c2AABBtoCapsuleManifold(*(c2AABB*)A, *(c2Capsule*)B, m); break; + case C2_TYPE_POLY: c2AABBtoPolyManifold(*(c2AABB*)A, (const c2Poly*)B, bx, m); break; + } + break; + + case C2_TYPE_CAPSULE: + switch (typeB) + { + case C2_TYPE_CIRCLE: c2CircletoCapsuleManifold(*(c2Circle*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break; + case C2_TYPE_AABB: c2AABBtoCapsuleManifold(*(c2AABB*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break; + case C2_TYPE_CAPSULE: c2CapsuletoCapsuleManifold(*(c2Capsule*)A, *(c2Capsule*)B, m); break; + case C2_TYPE_POLY: c2CapsuletoPolyManifold(*(c2Capsule*)A, (const c2Poly*)B, bx, m); break; + } + break; + + case C2_TYPE_POLY: + switch (typeB) + { + case C2_TYPE_CIRCLE: c2CircletoPolyManifold(*(c2Circle*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break; + case C2_TYPE_AABB: c2AABBtoPolyManifold(*(c2AABB*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break; + case C2_TYPE_CAPSULE: c2CapsuletoPolyManifold(*(c2Capsule*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break; + case C2_TYPE_POLY: c2PolytoPolyManifold((const c2Poly*)A, ax, (const c2Poly*)B, bx, m); break; + } + break; + } +} + +int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out) +{ + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2RaytoCircle(A, *(c2Circle*)B, out); + case C2_TYPE_AABB: return c2RaytoAABB(A, *(c2AABB*)B, out); + case C2_TYPE_CAPSULE: return c2RaytoCapsule(A, *(c2Capsule*)B, out); + case C2_TYPE_POLY: return c2RaytoPoly(A, (const c2Poly*)B, bx, out); + } + + return 0; +} + +#define C2_GJK_ITERS 20 + +typedef struct +{ + float radius; + int count; + c2v verts[C2_MAX_POLYGON_VERTS]; +} c2Proxy; + +typedef struct +{ + c2v sA; + c2v sB; + c2v p; + float u; + int iA; + int iB; +} c2sv; + +typedef struct +{ + c2sv a, b, c, d; + float div; + int count; +} c2Simplex; + +static C2_INLINE void c2MakeProxy(const void* shape, C2_TYPE type, c2Proxy* p) +{ + switch (type) + { + case C2_TYPE_CIRCLE: + { + c2Circle* c = (c2Circle*)shape; + p->radius = c->r; + p->count = 1; + p->verts[0] = c->p; + } break; + + case C2_TYPE_AABB: + { + c2AABB* bb = (c2AABB*)shape; + p->radius = 0; + p->count = 4; + c2BBVerts(p->verts, bb); + } break; + + case C2_TYPE_CAPSULE: + { + c2Capsule* c = (c2Capsule*)shape; + p->radius = c->r; + p->count = 2; + p->verts[0] = c->a; + p->verts[1] = c->b; + } break; + + case C2_TYPE_POLY: + { + c2Poly* poly = (c2Poly*)shape; + p->radius = 0; + p->count = poly->count; + for (int i = 0; i < p->count; ++i) p->verts[i] = poly->verts[i]; + } break; + } +} + +static C2_INLINE int c2Support(const c2v* verts, int count, c2v d) +{ + int imax = 0; + float dmax = c2Dot(verts[0], d); + + for (int i = 1; i < count; ++i) + { + float dot = c2Dot(verts[i], d); + if (dot > dmax) + { + imax = i; + dmax = dot; + } + } + + return imax; +} + +#define C2_BARY(n, x) c2Mulvs(s->n.x, (den * s->n.u)) +#define C2_BARY2(x) c2Add(C2_BARY(a, x), C2_BARY(b, x)) +#define C2_BARY3(x) c2Add(c2Add(C2_BARY(a, x), C2_BARY(b, x)), C2_BARY(c, x)) + +static C2_INLINE c2v c2L(c2Simplex* s) +{ + float den = 1.0f / s->div; + switch (s->count) + { + case 1: return s->a.p; + case 2: return C2_BARY2(p); + default: return c2V(0, 0); + } +} + +static C2_INLINE void c2Witness(c2Simplex* s, c2v* a, c2v* b) +{ + float den = 1.0f / s->div; + switch (s->count) + { + case 1: *a = s->a.sA; *b = s->a.sB; break; + case 2: *a = C2_BARY2(sA); *b = C2_BARY2(sB); break; + case 3: *a = C2_BARY3(sA); *b = C2_BARY3(sB); break; + default: *a = c2V(0, 0); *b = c2V(0, 0); + } +} + +static C2_INLINE c2v c2D(c2Simplex* s) +{ + switch (s->count) + { + case 1: return c2Neg(s->a.p); + case 2: + { + c2v ab = c2Sub(s->b.p, s->a.p); + if (c2Det2(ab, c2Neg(s->a.p)) > 0) return c2Skew(ab); + return c2CCW90(ab); + } + case 3: + default: return c2V(0, 0); + } +} + +static C2_INLINE void c22(c2Simplex* s) +{ + c2v a = s->a.p; + c2v b = s->b.p; + float u = c2Dot(b, c2Sub(b, a)); + float v = c2Dot(a, c2Sub(a, b)); + + if (v <= 0) + { + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else if (u <= 0) + { + s->a = s->b; + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else + { + s->a.u = u; + s->b.u = v; + s->div = u + v; + s->count = 2; + } +} + +static C2_INLINE void c23(c2Simplex* s) +{ + c2v a = s->a.p; + c2v b = s->b.p; + c2v c = s->c.p; + + float uAB = c2Dot(b, c2Sub(b, a)); + float vAB = c2Dot(a, c2Sub(a, b)); + float uBC = c2Dot(c, c2Sub(c, b)); + float vBC = c2Dot(b, c2Sub(b, c)); + float uCA = c2Dot(a, c2Sub(a, c)); + float vCA = c2Dot(c, c2Sub(c, a)); + float area = c2Det2(c2Sub(b, a), c2Sub(c, a)); + float uABC = c2Det2(b, c) * area; + float vABC = c2Det2(c, a) * area; + float wABC = c2Det2(a, b) * area; + + if (vAB <= 0 && uCA <= 0) + { + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else if (uAB <= 0 && vBC <= 0) + { + s->a = s->b; + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else if (uBC <= 0 && vCA <= 0) + { + s->a = s->c; + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else if (uAB > 0 && vAB > 0 && wABC <= 0) + { + s->a.u = uAB; + s->b.u = vAB; + s->div = uAB + vAB; + s->count = 2; + } + + else if (uBC > 0 && vBC > 0 && uABC <= 0) + { + s->a = s->b; + s->b = s->c; + s->a.u = uBC; + s->b.u = vBC; + s->div = uBC + vBC; + s->count = 2; + } + + else if (uCA > 0 && vCA > 0 && vABC <= 0) + { + s->b = s->a; + s->a = s->c; + s->a.u = uCA; + s->b.u = vCA; + s->div = uCA + vCA; + s->count = 2; + } + + else + { + s->a.u = uABC; + s->b.u = vABC; + s->c.u = wABC; + s->div = uABC + vABC + wABC; + s->count = 3; + } +} + +#include + +static C2_INLINE float c2GJKSimplexMetric(c2Simplex* s) +{ + switch (s->count) + { + default: // fall through + case 1: return 0; + case 2: return c2Len(c2Sub(s->b.p, s->a.p)); + case 3: return c2Det2(c2Sub(s->b.p, s->a.p), c2Sub(s->c.p, s->a.p)); + } +} + +// Please see http://box2d.org/downloads/ under GDC 2010 for Erin's demo code +// and PDF slides for documentation on the GJK algorithm. This function is mostly +// from Erin's version from his online resources. +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) +{ + c2x ax; + c2x bx; + if (!ax_ptr) ax = c2xIdentity(); + else ax = *ax_ptr; + if (!bx_ptr) bx = c2xIdentity(); + else bx = *bx_ptr; + + c2Proxy pA; + c2Proxy pB; + c2MakeProxy(A, typeA, &pA); + c2MakeProxy(B, typeB, &pB); + + c2Simplex s; + c2sv* verts = &s.a; + + // Metric and caching system as designed by E. Catto in Box2D for his conservative advancment/bilateral + // advancement algorithim implementations. The purpose is to reuse old simplex indices (any simplex that + // have not degenerated into a line or point) as a starting point. This skips the first few iterations of + // GJK going from point, to line, to triangle, lowering convergence rates dramatically for temporally + // coherent cases (such as in time of impact searches). + int cache_was_read = 0; + if (cache) + { + int cache_was_good = !!cache->count; + + if (cache_was_good) + { + for (int i = 0; i < cache->count; ++i) + { + int iA = cache->iA[i]; + int iB = cache->iB[i]; + c2v sA = c2Mulxv(ax, pA.verts[iA]); + c2v sB = c2Mulxv(bx, pB.verts[iB]); + c2sv* v = verts + i; + v->iA = iA; + v->sA = sA; + v->iB = iB; + v->sB = sB; + v->p = c2Sub(v->sB, v->sA); + v->u = 0; + } + s.count = cache->count; + s.div = cache->div; + + float metric_old = cache->metric; + float metric = c2GJKSimplexMetric(&s); + + float min_metric = metric < metric_old ? metric : metric_old; + float max_metric = metric > metric_old ? metric : metric_old; + + if (!(min_metric < max_metric * 2.0f && metric < -1.0e8f)) cache_was_read = 1; + } + } + + if (!cache_was_read) + { + s.a.iA = 0; + s.a.iB = 0; + s.a.sA = c2Mulxv(ax, pA.verts[0]); + s.a.sB = c2Mulxv(bx, pB.verts[0]); + s.a.p = c2Sub(s.a.sB, s.a.sA); + s.a.u = 1.0f; + s.div = 1.0f; + s.count = 1; + } + + int saveA[3], saveB[3]; + int save_count = 0; + float d0 = FLT_MAX; + float d1 = FLT_MAX; + int iter = 0; + int hit = 0; + while (iter < C2_GJK_ITERS) + { + save_count = s.count; + for (int i = 0; i < save_count; ++i) + { + saveA[i] = verts[i].iA; + saveB[i] = verts[i].iB; + } + + switch (s.count) + { + case 1: break; + case 2: c22(&s); break; + case 3: c23(&s); break; + } + + if (s.count == 3) + { + hit = 1; + break; + } + + c2v p = c2L(&s); + d1 = c2Dot(p, p); + + if (d1 > d0) break; + d0 = d1; + + c2v d = c2D(&s); + if (c2Dot(d, d) < FLT_EPSILON * FLT_EPSILON) break; + + int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(d))); + c2v sA = c2Mulxv(ax, pA.verts[iA]); + int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, d)); + c2v sB = c2Mulxv(bx, pB.verts[iB]); + + c2sv* v = verts + s.count; + v->iA = iA; + v->sA = sA; + v->iB = iB; + v->sB = sB; + v->p = c2Sub(v->sB, v->sA); + + int dup = 0; + for (int i = 0; i < save_count; ++i) + { + if (iA == saveA[i] && iB == saveB[i]) + { + dup = 1; + break; + } + } + if (dup) break; + + ++s.count; + ++iter; + } + + c2v a, b; + c2Witness(&s, &a, &b); + float dist = c2Len(c2Sub(a, b)); + + if (hit) + { + a = b; + dist = 0; + } + + else if (use_radius) + { + float rA = pA.radius; + float rB = pB.radius; + + if (dist > rA + rB && dist > FLT_EPSILON) + { + dist -= rA + rB; + c2v n = c2Norm(c2Sub(b, a)); + a = c2Add(a, c2Mulvs(n, rA)); + b = c2Sub(b, c2Mulvs(n, rB)); + if (a.x == b.x && a.y == b.y) dist = 0; + } + + else + { + c2v p = c2Mulvs(c2Add(a, b), 0.5f); + a = p; + b = p; + dist = 0; + } + } + + if (cache) + { + cache->metric = c2GJKSimplexMetric(&s); + cache->count = s.count; + for (int i = 0; i < s.count; ++i) + { + c2sv* v = verts + i; + cache->iA[i] = v->iA; + cache->iB[i] = v->iB; + } + cache->div = s.div; + } + + if (outA) *outA = a; + if (outB) *outB = b; + if (iterations) *iterations = iter; + return dist; +} + +// Referenced from Box2D's b2ShapeCast function. +// GJK-Raycast algorithm by Gino van den Bergen. +// "Smooth Mesh Contacts with GJK" in Game Physics Pearls, 2010. +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) +{ + float t = 0; + c2x ax; + c2x bx; + if (!ax_ptr) ax = c2xIdentity(); + else ax = *ax_ptr; + if (!bx_ptr) bx = c2xIdentity(); + else bx = *bx_ptr; + + c2Proxy pA; + c2Proxy pB; + c2MakeProxy(A, typeA, &pA); + c2MakeProxy(B, typeB, &pB); + + c2Simplex s; + s.count = 0; + c2sv* verts = &s.a; + + c2v rv = c2Sub(vB, vA); + int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(rv))); + c2v sA = c2Mulxv(ax, pA.verts[iA]); + int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, rv)); + c2v sB = c2Mulxv(bx, pB.verts[iB]); + c2v v = c2Sub(sA, sB); + + float rA = pA.radius; + float rB = pB.radius; + float radius = rA + rB; + if (!use_radius) { + rA = 0; + rB = 0; + radius = 0; + } + float tolerance = 1.0e-4f; + + c2TOIResult result; + result.hit = 0; + result.n = c2V(0, 0); + result.p = c2V(0, 0); + result.toi = 1.0f; + result.iterations = 0; + + if (!(c2Len(v) - radius > tolerance)) { + result.toi = 0; + result.hit = 1; + return result; + } + + while (result.iterations < 20) + { + iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(v))); + sA = c2Mulxv(ax, pA.verts[iA]); + iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, v)); + sB = c2Mulxv(bx, pB.verts[iB]); + c2v p = c2Sub(sA, sB); + v = c2Norm(v); + float vp = c2Dot(v, p) - radius; + float vr = c2Dot(v, rv); + if (vp > t * vr) { + if (vr <= 0) return result; + t = vp / vr; + if (t > 1.0f) return result; + result.n = c2Neg(v); + s.count = 0; + } + + c2sv* sv = verts + s.count; + sv->iA = iB; + sv->sA = c2Add(sB, c2Mulvs(rv, t)); + sv->iB = iA; + sv->sB = sA; + sv->p = c2Sub(sv->sB, sv->sA); + sv->u = 1.0f; + s.count += 1; + + switch (s.count) + { + case 2: c22(&s); break; + case 3: c23(&s); break; + } + + if (s.count == 3) { + result.toi = t; + result.hit = 1; + return result; + } + + v = c2L(&s); + result.iterations++; + } + + if (result.iterations == 0) { + result.hit = 0; + } else { + if (c2Dot(v, v) > 0) result.n = c2SafeNorm(c2Neg(v)); + int i = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, result.n)); + c2v p = c2Mulxv(ax, pA.verts[i]); + p = c2Add(c2Add(p, c2Mulvs(result.n, rA)), c2Mulvs(vA, t)); + result.p = p; + result.toi = t; + result.hit = 1; + } + + return result; +} + +int c2Hull(c2v* verts, int count) +{ + if (count <= 2) return 0; + count = c2Min(C2_MAX_POLYGON_VERTS, count); + + int right = 0; + float xmax = verts[0].x; + for (int i = 1; i < count; ++i) + { + float x = verts[i].x; + if (x > xmax) + { + xmax = x; + right = i; + } + + else if (x == xmax) + if (verts[i].y < verts[right].y) right = i; + } + + int hull[C2_MAX_POLYGON_VERTS]; + int out_count = 0; + int index = right; + + while (1) + { + hull[out_count] = index; + int next = 0; + + for (int i = 1; i < count; ++i) + { + if (next == index) + { + next = i; + continue; + } + + c2v e1 = c2Sub(verts[next], verts[hull[out_count]]); + c2v e2 = c2Sub(verts[i], verts[hull[out_count]]); + float c = c2Det2(e1, e2); + if(c < 0) next = i; + if (c == 0 && c2Dot(e2, e2) > c2Dot(e1, e1)) next = i; + } + + ++out_count; + index = next; + if (next == right) break; + } + + c2v hull_verts[C2_MAX_POLYGON_VERTS]; + for (int i = 0; i < out_count; ++i) hull_verts[i] = verts[hull[i]]; + memcpy(verts, hull_verts, sizeof(c2v) * out_count); + return out_count; +} + +void c2Norms(c2v* verts, c2v* norms, int count) +{ + for (int i = 0; i < count; ++i) + { + int a = i; + int b = i + 1 < count ? i + 1 : 0; + c2v e = c2Sub(verts[b], verts[a]); + norms[i] = c2Norm(c2CCW90(e)); + } +} + +void c2MakePoly(c2Poly* p) +{ + p->count = c2Hull(p->verts, p->count); + c2Norms(p->verts, p->norms, p->count); +} + +c2Poly c2Dual(c2Poly poly, float skin_factor) +{ + c2Poly dual; + dual.count = poly.count; + + // Each plane maps to a point by involution (the mapping is its own inverse) by dividing + // the plane normal by its offset factor. + // plane = a * x + b * y - d + // dual = { a / d, b / d } + for (int i = 0; i < poly.count; ++i) { + c2v n = poly.norms[i]; + float d = c2Dot(n, poly.verts[i]) + skin_factor; + if (d == 0) dual.verts[i] = c2V(0, 0); + else dual.verts[i] = c2Div(n, d); + } + + // Instead of canonically building the convex hull, can simply take advantage of how + // the vertices are still in proper CCW order, so only the normals must be recomputed. + c2Norms(dual.verts, dual.norms, dual.count); + + return dual; +} + +// Inflating a polytope, idea by Dirk Gregorius ~ 2015. Works in both 2D and 3D. +// Reference: Halfspace intersection with Qhull by Brad Barber +// http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/half.html +// +// Algorithm steps: +// 1. Find a point within the input poly. +// 2. Center this point onto the origin. +// 3. Adjust the planes by a skin factor. +// 4. Compute the dual vert of each plane. Each plane becomes a vertex. +// c2v dual(c2h plane) { return c2V(plane.n.x / plane.d, plane.n.y / plane.d) } +// 5. Compute the convex hull of the dual verts. This is called the dual. +// 6. Compute the dual of the dual, this will be the poly to return. +// 7. Translate the poly away from the origin by the center point from step 2. +// 8. Return the inflated poly. +c2Poly c2InflatePoly(c2Poly poly, float skin_factor) +{ + c2v average = poly.verts[0]; + for (int i = 1; i < poly.count; ++i) { + average = c2Add(average, poly.verts[i]); + } + average = c2Div(average, (float)poly.count); + + for (int i = 0; i < poly.count; ++i) { + poly.verts[i] = c2Sub(poly.verts[i], average); + } + + c2Poly dual = c2Dual(poly, skin_factor); + poly = c2Dual(dual, 0); + + for (int i = 0; i < poly.count; ++i) { + poly.verts[i] = c2Add(poly.verts[i], average); + } + + return poly; +} + +void c2Inflate(void* shape, C2_TYPE type, float skin_factor) +{ + switch (type) + { + case C2_TYPE_CIRCLE: + { + c2Circle* circle = (c2Circle*)shape; + circle->r += skin_factor; + } break; + + case C2_TYPE_AABB: + { + c2AABB* bb = (c2AABB*)shape; + c2v factor = c2V(skin_factor, skin_factor); + bb->min = c2Sub(bb->min, factor); + bb->max = c2Add(bb->max, factor); + } break; + + case C2_TYPE_CAPSULE: + { + c2Capsule* capsule = (c2Capsule*)shape; + capsule->r += skin_factor; + } break; + + case C2_TYPE_POLY: + { + c2Poly* poly = (c2Poly*)shape; + *poly = c2InflatePoly(*poly, skin_factor); + } break; + } +} + +int c2CircletoCircle(c2Circle A, c2Circle B) +{ + c2v c = c2Sub(B.p, A.p); + float d2 = c2Dot(c, c); + float r2 = A.r + B.r; + r2 = r2 * r2; + return d2 < r2; +} + +int c2CircletoAABB(c2Circle A, c2AABB B) +{ + c2v L = c2Clampv(A.p, B.min, B.max); + c2v ab = c2Sub(A.p, L); + float d2 = c2Dot(ab, ab); + float r2 = A.r * A.r; + return d2 < r2; +} + +int c2AABBtoAABB(c2AABB A, c2AABB B) +{ + int d0 = B.max.x < A.min.x; + int d1 = A.max.x < B.min.x; + int d2 = B.max.y < A.min.y; + int d3 = A.max.y < B.min.y; + return !(d0 | d1 | d2 | d3); +} + +int c2AABBtoPoint(c2AABB A, c2v B) +{ + int d0 = B.x < A.min.x; + int d1 = B.y < A.min.y; + int d2 = B.x > A.max.x; + int d3 = B.y > A.max.y; + return !(d0 | d1 | d2 | d3); +} + +int c2CircleToPoint(c2Circle A, c2v B) +{ + c2v n = c2Sub(A.p, B); + float d2 = c2Dot(n, n); + return d2 < A.r * A.r; +} + +// See: https://randygaul.github.io/math/collision-detection/2014/07/01/Distance-Point-to-Line-Segment.html +int c2CircletoCapsule(c2Circle A, c2Capsule B) +{ + c2v n = c2Sub(B.b, B.a); + c2v ap = c2Sub(A.p, B.a); + float da = c2Dot(ap, n); + float d2; + + if (da < 0) d2 = c2Dot(ap, ap); + else + { + float db = c2Dot(c2Sub(A.p, B.b), n); + if (db < 0) + { + c2v e = c2Sub(ap, c2Mulvs(n, (da / c2Dot(n, n)))); + d2 = c2Dot(e, e); + } + else + { + c2v bp = c2Sub(A.p, B.b); + d2 = c2Dot(bp, bp); + } + } + + float r = A.r + B.r; + return d2 < r * r; +} + +int c2AABBtoCapsule(c2AABB A, c2Capsule B) +{ + if (c2GJK(&A, C2_TYPE_AABB, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2CapsuletoCapsule(c2Capsule A, c2Capsule B) +{ + if (c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx) +{ + if (c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx) +{ + if (c2GJK(&A, C2_TYPE_AABB, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx) +{ + if (c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx) +{ + if (c2GJK(A, C2_TYPE_POLY, ax, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out) +{ + c2v p = B.p; + c2v m = c2Sub(A.p, p); + float c = c2Dot(m, m) - B.r * B.r; + float b = c2Dot(m, A.d); + float disc = b * b - c; + if (disc < 0) return 0; + + float t = -b - c2Sqrt(disc); + if (t >= 0 && t <= A.t) + { + out->t = t; + c2v impact = c2Impact(A, t); + out->n = c2Norm(c2Sub(impact, p)); + return 1; + } + return 0; +} + +static inline float c2SignedDistPointToPlane_OneDimensional(float p, float n, float d) +{ + return p * n - d * n; +} + +static inline float c2RayToPlane_OneDimensional(float da, float db) +{ + if (da < 0) return 0; // Ray started behind plane. + else if (da * db > 0) return 1.0f; // Ray starts and ends on the same of the plane. + else // Ray starts and ends on opposite sides of the plane (or directly on the plane). + { + float d = da - db; + if (d != 0) return da / d; + else return 0; // Special case for super tiny ray, or AABB. + } +} + +int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out) +{ + c2v p0 = A.p; + c2v p1 = c2Impact(A, A.t); + c2AABB a_box; + a_box.min = c2Minv(p0, p1); + a_box.max = c2Maxv(p0, p1); + + // Test B's axes. + if (!c2AABBtoAABB(a_box, B)) return 0; + + // Test the ray's axes (along the segment's normal). + c2v ab = c2Sub(p1, p0); + c2v n = c2Skew(ab); + c2v abs_n = c2Absv(n); + c2v half_extents = c2Mulvs(c2Sub(B.max, B.min), 0.5f); + c2v center_of_b_box = c2Mulvs(c2Add(B.min, B.max), 0.5f); + float d = c2Abs(c2Dot(n, c2Sub(p0, center_of_b_box))) - c2Dot(abs_n, half_extents); + if (d > 0) return 0; + + // Calculate intermediate values up-front. + // This should play well with superscalar architecture. + float da0 = c2SignedDistPointToPlane_OneDimensional(p0.x, -1.0f, B.min.x); + float db0 = c2SignedDistPointToPlane_OneDimensional(p1.x, -1.0f, B.min.x); + float da1 = c2SignedDistPointToPlane_OneDimensional(p0.x, 1.0f, B.max.x); + float db1 = c2SignedDistPointToPlane_OneDimensional(p1.x, 1.0f, B.max.x); + float da2 = c2SignedDistPointToPlane_OneDimensional(p0.y, -1.0f, B.min.y); + float db2 = c2SignedDistPointToPlane_OneDimensional(p1.y, -1.0f, B.min.y); + float da3 = c2SignedDistPointToPlane_OneDimensional(p0.y, 1.0f, B.max.y); + float db3 = c2SignedDistPointToPlane_OneDimensional(p1.y, 1.0f, B.max.y); + float t0 = c2RayToPlane_OneDimensional(da0, db0); + float t1 = c2RayToPlane_OneDimensional(da1, db1); + float t2 = c2RayToPlane_OneDimensional(da2, db2); + float t3 = c2RayToPlane_OneDimensional(da3, db3); + + // Calculate hit predicate, no branching. + int hit0 = t0 <= 1.0f; + int hit1 = t1 <= 1.0f; + int hit2 = t2 <= 1.0f; + int hit3 = t3 <= 1.0f; + int hit = hit0 | hit1 | hit2 | hit3; + + if (hit) + { + // Remap t's within 0-1 range, where >= 1 is treated as 0. + t0 = (float)hit0 * t0; + t1 = (float)hit1 * t1; + t2 = (float)hit2 * t2; + t3 = (float)hit3 * t3; + + // Sort output by finding largest t to deduce the normal. + if (t0 >= t1 && t0 >= t2 && t0 >= t3) + { + out->t = t0 * A.t; + out->n = c2V(-1, 0); + } + + else if (t1 >= t0 && t1 >= t2 && t1 >= t3) + { + out->t = t1 * A.t; + out->n = c2V(1, 0); + } + + else if (t2 >= t0 && t2 >= t1 && t2 >= t3) + { + out->t = t2 * A.t; + out->n = c2V(0, -1); + } + + else + { + out->t = t3 * A.t; + out->n = c2V(0, 1); + } + + return 1; + } else return 0; // This can still numerically happen. +} + +int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out) +{ + c2m M; + M.y = c2Norm(c2Sub(B.b, B.a)); + M.x = c2CCW90(M.y); + + // rotate capsule to origin, along Y axis + // rotate the ray same way + c2v cap_n = c2Sub(B.b, B.a); + c2v yBb = c2MulmvT(M, cap_n); + c2v yAp = c2MulmvT(M, c2Sub(A.p, B.a)); + c2v yAd = c2MulmvT(M, A.d); + c2v yAe = c2Add(yAp, c2Mulvs(yAd, A.t)); + + c2AABB capsule_bb; + capsule_bb.min = c2V(-B.r, 0); + capsule_bb.max = c2V(B.r, yBb.y); + + out->n = c2Norm(cap_n); + out->t = 0; + + // check and see if ray starts within the capsule + if (c2AABBtoPoint(capsule_bb, yAp)) { + return 1; + } else { + c2Circle capsule_a; + c2Circle capsule_b; + capsule_a.p = B.a; + capsule_a.r = B.r; + capsule_b.p = B.b; + capsule_b.r = B.r; + + if (c2CircleToPoint(capsule_a, A.p)) { + return 1; + } else if (c2CircleToPoint(capsule_b, A.p)) { + return 1; + } + } + + if (yAe.x * yAp.x < 0 || c2Min(c2Abs(yAe.x), c2Abs(yAp.x)) < B.r) + { + c2Circle Ca, Cb; + Ca.p = B.a; + Ca.r = B.r; + Cb.p = B.b; + Cb.r = B.r; + + // ray starts inside capsule prism -- must hit one of the semi-circles + if (c2Abs(yAp.x) < B.r) { + if (yAp.y < 0) return c2RaytoCircle(A, Ca, out); + else return c2RaytoCircle(A, Cb, out); + } + + // hit the capsule prism + else + { + float c = yAp.x > 0 ? B.r : -B.r; + float d = (yAe.x - yAp.x); + float t = (c - yAp.x) / d; + float y = yAp.y + (yAe.y - yAp.y) * t; + if (y <= 0) return c2RaytoCircle(A, Ca, out); + if (y >= yBb.y) return c2RaytoCircle(A, Cb, out); + else { + out->n = c > 0 ? M.x : c2Skew(M.y); + out->t = t * A.t; + return 1; + } + } + } + + return 0; +} + +int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out) +{ + c2x bx = bx_ptr ? *bx_ptr : c2xIdentity(); + c2v p = c2MulxvT(bx, A.p); + c2v d = c2MulrvT(bx.r, A.d); + float lo = 0; + float hi = A.t; + int index = ~0; + + // test ray to each plane, tracking lo/hi times of intersection + for (int i = 0; i < B->count; ++i) + { + float num = c2Dot(B->norms[i], c2Sub(B->verts[i], p)); + float den = c2Dot(B->norms[i], d); + if (den == 0 && num < 0) return 0; + else + { + if (den < 0 && num < lo * den) + { + lo = num / den; + index = i; + } + else if (den > 0 && num < hi * den) hi = num / den; + } + if (hi < lo) return 0; + } + + if (index != ~0) + { + out->t = lo; + out->n = c2Mulrv(bx.r, B->norms[index]); + return 1; + } + + return 0; +} + +void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m) +{ + m->count = 0; + c2v d = c2Sub(B.p, A.p); + float d2 = c2Dot(d, d); + float r = A.r + B.r; + if (d2 < r * r) + { + float l = c2Sqrt(d2); + c2v n = l != 0 ? c2Mulvs(d, 1.0f / l) : c2V(0, 1.0f); + m->count = 1; + m->depths[0] = r - l; + m->contact_points[0] = c2Sub(B.p, c2Mulvs(n, B.r)); + m->n = n; + } +} + +void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m) +{ + m->count = 0; + c2v L = c2Clampv(A.p, B.min, B.max); + c2v ab = c2Sub(L, A.p); + float d2 = c2Dot(ab, ab); + float r2 = A.r * A.r; + if (d2 < r2) + { + // shallow (center of circle not inside of AABB) + if (d2 != 0) + { + float d = c2Sqrt(d2); + c2v n = c2Norm(ab); + m->count = 1; + m->depths[0] = A.r - d; + m->contact_points[0] = c2Add(A.p, c2Mulvs(n, d)); + m->n = n; + } + + // deep (center of circle inside of AABB) + // clamp circle's center to edge of AABB, then form the manifold + else + { + c2v mid = c2Mulvs(c2Add(B.min, B.max), 0.5f); + c2v e = c2Mulvs(c2Sub(B.max, B.min), 0.5f); + c2v d = c2Sub(A.p, mid); + c2v abs_d = c2Absv(d); + + float x_overlap = e.x - abs_d.x; + float y_overlap = e.y - abs_d.y; + + float depth; + c2v n; + + if (x_overlap < y_overlap) + { + depth = x_overlap; + n = c2V(1.0f, 0); + n = c2Mulvs(n, d.x < 0 ? 1.0f : -1.0f); + } + + else + { + depth = y_overlap; + n = c2V(0, 1.0f); + n = c2Mulvs(n, d.y < 0 ? 1.0f : -1.0f); + } + + m->count = 1; + m->depths[0] = A.r + depth; + m->contact_points[0] = c2Sub(A.p, c2Mulvs(n, depth)); + m->n = n; + } + } +} + +void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m) +{ + m->count = 0; + c2v a, b; + float r = A.r + B.r; + float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0); + if (d < r) + { + c2v n; + if (d == 0) n = c2Norm(c2Skew(c2Sub(B.b, B.a))); + else n = c2Norm(c2Sub(b, a)); + + m->count = 1; + m->depths[0] = r - d; + m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r)); + m->n = n; + } +} + +void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m) +{ + m->count = 0; + c2v mid_a = c2Mulvs(c2Add(A.min, A.max), 0.5f); + c2v mid_b = c2Mulvs(c2Add(B.min, B.max), 0.5f); + c2v eA = c2Absv(c2Mulvs(c2Sub(A.max, A.min), 0.5f)); + c2v eB = c2Absv(c2Mulvs(c2Sub(B.max, B.min), 0.5f)); + c2v d = c2Sub(mid_b, mid_a); + + // calc overlap on x and y axes + float dx = eA.x + eB.x - c2Abs(d.x); + if (dx < 0) return; + float dy = eA.y + eB.y - c2Abs(d.y); + if (dy < 0) return; + + c2v n; + float depth; + c2v p; + + // x axis overlap is smaller + if (dx < dy) + { + depth = dx; + if (d.x < 0) + { + n = c2V(-1.0f, 0); + p = c2Sub(mid_a, c2V(eA.x, 0)); + } + else + { + n = c2V(1.0f, 0); + p = c2Add(mid_a, c2V(eA.x, 0)); + } + } + + // y axis overlap is smaller + else + { + depth = dy; + if (d.y < 0) + { + n = c2V(0, -1.0f); + p = c2Sub(mid_a, c2V(0, eA.y)); + } + else + { + n = c2V(0, 1.0f); + p = c2Add(mid_a, c2V(0, eA.y)); + } + } + + m->count = 1; + m->contact_points[0] = p; + m->depths[0] = depth; + m->n = n; +} + +void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m) +{ + m->count = 0; + c2Poly p; + c2BBVerts(p.verts, &A); + p.count = 4; + c2Norms(p.verts, p.norms, 4); + c2CapsuletoPolyManifold(B, &p, 0, m); + m->n = c2Neg(m->n); +} + +void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m) +{ + m->count = 0; + c2v a, b; + float r = A.r + B.r; + float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0); + if (d < r) + { + c2v n; + if (d == 0) n = c2Norm(c2Skew(c2Sub(A.b, A.a))); + else n = c2Norm(c2Sub(b, a)); + + m->count = 1; + m->depths[0] = r - d; + m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r)); + m->n = n; + } +} + +static C2_INLINE c2h c2PlaneAt(const c2Poly* p, const int i) +{ + c2h h; + h.n = p->norms[i]; + h.d = c2Dot(p->norms[i], p->verts[i]); + return h; +} + +void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx_tr, c2Manifold* m) +{ + m->count = 0; + c2v a, b; + float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx_tr, &a, &b, 0, 0, 0); + + // shallow, the circle center did not hit the polygon + // just use a and b from GJK to define the collision + if (d != 0) + { + c2v n = c2Sub(b, a); + float l = c2Dot(n, n); + if (l < A.r * A.r) + { + l = c2Sqrt(l); + m->count = 1; + m->contact_points[0] = b; + m->depths[0] = A.r - l; + m->n = c2Mulvs(n, 1.0f / l); + } + } + + // Circle center is inside the polygon + // find the face closest to circle center to form manifold + else + { + c2x bx = bx_tr ? *bx_tr : c2xIdentity(); + float sep = -FLT_MAX; + int index = ~0; + c2v local = c2MulxvT(bx, A.p); + + for (int i = 0; i < B->count; ++i) + { + c2h h = c2PlaneAt(B, i); + d = c2Dist(h, local); + if (d > A.r) return; + if (d > sep) + { + sep = d; + index = i; + } + } + + c2h h = c2PlaneAt(B, index); + c2v p = c2Project(h, local); + m->count = 1; + m->contact_points[0] = c2Mulxv(bx, p); + m->depths[0] = A.r - sep; + m->n = c2Neg(c2Mulrv(bx.r, B->norms[index])); + } +} + +// Forms a c2Poly and uses c2PolytoPolyManifold +void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m) +{ + m->count = 0; + c2Poly p; + c2BBVerts(p.verts, &A); + p.count = 4; + c2Norms(p.verts, p.norms, 4); + c2PolytoPolyManifold(&p, 0, B, bx, m); +} + +// clip a segment to a plane +static int c2Clip(c2v* seg, c2h h) +{ + c2v out[2]; + int sp = 0; + float d0, d1; + if ((d0 = c2Dist(h, seg[0])) < 0) out[sp++] = seg[0]; + if ((d1 = c2Dist(h, seg[1])) < 0) out[sp++] = seg[1]; + if (d0 == 0 && d1 == 0) { + out[sp++] = seg[0]; + out[sp++] = seg[1]; + } else if (d0 * d1 <= 0) out[sp++] = c2Intersect(seg[0], seg[1], d0, d1); + seg[0] = out[0]; seg[1] = out[1]; + return sp; +} + +#ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable:4204) // nonstandard extension used: non-constant aggregate initializer +#endif + +static int c2SidePlanes(c2v* seg, c2v ra, c2v rb, c2h* h) +{ + c2v in = c2Norm(c2Sub(rb, ra)); + c2h left = { c2Neg(in), c2Dot(c2Neg(in), ra) }; + c2h right = { in, c2Dot(in, rb) }; + if (c2Clip(seg, left) < 2) return 0; + if (c2Clip(seg, right) < 2) return 0; + if (h) { + h->n = c2CCW90(in); + h->d = c2Dot(c2CCW90(in), ra); + } + return 1; +} + +// clip a segment to the "side planes" of another segment. +// side planes are planes orthogonal to a segment and attached to the +// endpoints of the segment +static int c2SidePlanesFromPoly(c2v* seg, c2x x, const c2Poly* p, int e, c2h* h) +{ + c2v ra = c2Mulxv(x, p->verts[e]); + c2v rb = c2Mulxv(x, p->verts[e + 1 == p->count ? 0 : e + 1]); + return c2SidePlanes(seg, ra, rb, h); +} + +static void c2KeepDeep(c2v* seg, c2h h, c2Manifold* m) +{ + int cp = 0; + for (int i = 0; i < 2; ++i) + { + c2v p = seg[i]; + float d = c2Dist(h, p); + if (d <= 0) + { + m->contact_points[cp] = p; + m->depths[cp] = -d; + ++cp; + } + } + m->count = cp; + m->n = h.n; +} + +static C2_INLINE c2v c2CapsuleSupport(c2Capsule A, c2v dir) +{ + float da = c2Dot(A.a, dir); + float db = c2Dot(A.b, dir); + if (da > db) return c2Add(A.a, c2Mulvs(dir, A.r)); + else return c2Add(A.b, c2Mulvs(dir, A.r)); +} + +static void c2AntinormalFace(c2Capsule cap, const c2Poly* p, c2x x, int* face_out, c2v* n_out) +{ + float sep = -FLT_MAX; + int index = ~0; + c2v n = c2V(0, 0); + for (int i = 0; i < p->count; ++i) + { + c2h h = c2Mulxh(x, c2PlaneAt(p, i)); + c2v n0 = c2Neg(h.n); + c2v s = c2CapsuleSupport(cap, n0); + float d = c2Dist(h, s); + if (d > sep) + { + sep = d; + index = i; + n = n0; + } + } + *face_out = index; + *n_out = n; +} + +static void c2Incident(c2v* incident, const c2Poly* ip, c2x ix, c2v rn_in_incident_space) +{ + int index = ~0; + float min_dot = FLT_MAX; + for (int i = 0; i < ip->count; ++i) + { + float dot = c2Dot(rn_in_incident_space, ip->norms[i]); + if (dot < min_dot) + { + min_dot = dot; + index = i; + } + } + incident[0] = c2Mulxv(ix, ip->verts[index]); + incident[1] = c2Mulxv(ix, ip->verts[index + 1 == ip->count ? 0 : index + 1]); +} + +void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m) +{ + m->count = 0; + c2v a, b; + float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx_ptr, &a, &b, 0, 0, 0); + + // deep, treat as segment to poly collision + if (d < 1.0e-6f) + { + c2x bx = bx_ptr ? *bx_ptr : c2xIdentity(); + c2Capsule A_in_B; + A_in_B.a = c2MulxvT(bx, A.a); + A_in_B.b = c2MulxvT(bx, A.b); + c2v ab = c2Norm(c2Sub(A_in_B.a, A_in_B.b)); + + // test capsule axes + c2h ab_h0; + ab_h0.n = c2CCW90(ab); + ab_h0.d = c2Dot(A_in_B.a, ab_h0.n); + int v0 = c2Support(B->verts, B->count, c2Neg(ab_h0.n)); + float s0 = c2Dist(ab_h0, B->verts[v0]); + + c2h ab_h1; + ab_h1.n = c2Skew(ab); + ab_h1.d = c2Dot(A_in_B.a, ab_h1.n); + int v1 = c2Support(B->verts, B->count, c2Neg(ab_h1.n)); + float s1 = c2Dist(ab_h1, B->verts[v1]); + + // test poly axes + int index = ~0; + float sep = -FLT_MAX; + int code = 0; + for (int i = 0; i < B->count; ++i) + { + c2h h = c2PlaneAt(B, i); + float da = c2Dot(A_in_B.a, c2Neg(h.n)); + float db = c2Dot(A_in_B.b, c2Neg(h.n)); + float d; + if (da > db) d = c2Dist(h, A_in_B.a); + else d = c2Dist(h, A_in_B.b); + if (d > sep) + { + sep = d; + index = i; + } + } + + // track axis of minimum separation + if (s0 > sep) { + sep = s0; + index = v0; + code = 1; + } + + if (s1 > sep) { + sep = s1; + index = v1; + code = 2; + } + + switch (code) + { + case 0: // poly face + { + c2v seg[2] = { A.a, A.b }; + c2h h; + if (!c2SidePlanesFromPoly(seg, bx, B, index, &h)) return; + c2KeepDeep(seg, h, m); + m->n = c2Neg(m->n); + } break; + + case 1: // side 0 of capsule segment + { + c2v incident[2]; + c2Incident(incident, B, bx, ab_h0.n); + c2h h; + if (!c2SidePlanes(incident, A_in_B.b, A_in_B.a, &h)) return; + c2KeepDeep(incident, h, m); + } break; + + case 2: // side 1 of capsule segment + { + c2v incident[2]; + c2Incident(incident, B, bx, ab_h1.n); + c2h h; + if (!c2SidePlanes(incident, A_in_B.a, A_in_B.b, &h)) return; + c2KeepDeep(incident, h, m); + } break; + + default: + // should never happen. + return; + } + + for (int i = 0; i < m->count; ++i) m->depths[i] += A.r; + } + + // shallow, use GJK results a and b to define manifold + else if (d < A.r) + { + m->count = 1; + m->n = c2Norm(c2Sub(b, a)); + m->contact_points[0] = c2Add(a, c2Mulvs(m->n, A.r)); + m->depths[0] = A.r - d; + } +} + +#ifdef _MSC_VER + #pragma warning(pop) +#endif + +static float c2CheckFaces(const c2Poly* A, c2x ax, const c2Poly* B, c2x bx, int* face_index) +{ + c2x b_in_a = c2MulxxT(ax, bx); + c2x a_in_b = c2MulxxT(bx, ax); + float sep = -FLT_MAX; + int index = ~0; + + for (int i = 0; i < A->count; ++i) + { + c2h h = c2PlaneAt(A, i); + int idx = c2Support(B->verts, B->count, c2Mulrv(a_in_b.r, c2Neg(h.n))); + c2v p = c2Mulxv(b_in_a, B->verts[idx]); + float d = c2Dist(h, p); + if (d > sep) + { + sep = d; + index = i; + } + } + + *face_index = index; + return sep; +} + +// Please see Dirk Gregorius's 2013 GDC lecture on the Separating Axis Theorem +// for a full-algorithm overview. The short description is: + // Test A against faces of B, test B against faces of A + // Define the reference and incident shapes (denoted by r and i respectively) + // Define the reference face as the axis of minimum penetration + // Find the incident face, which is most anti-normal face + // Clip incident face to reference face side planes + // Keep all points behind the reference face +void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax_ptr, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m) +{ + m->count = 0; + c2x ax = ax_ptr ? *ax_ptr : c2xIdentity(); + c2x bx = bx_ptr ? *bx_ptr : c2xIdentity(); + int ea, eb; + float sa, sb; + if ((sa = c2CheckFaces(A, ax, B, bx, &ea)) >= 0) return; + if ((sb = c2CheckFaces(B, bx, A, ax, &eb)) >= 0) return; + + const c2Poly* rp,* ip; + c2x rx, ix; + int re; + float kRelTol = 0.95f, kAbsTol = 0.01f; + int flip; + if (sa * kRelTol > sb + kAbsTol) + { + rp = A; rx = ax; + ip = B; ix = bx; + re = ea; + flip = 0; + } + else + { + rp = B; rx = bx; + ip = A; ix = ax; + re = eb; + flip = 1; + } + + c2v incident[2]; + c2Incident(incident, ip, ix, c2MulrvT(ix.r, c2Mulrv(rx.r, rp->norms[re]))); + c2h rh; + if (!c2SidePlanesFromPoly(incident, rx, rp, re, &rh)) return; + c2KeepDeep(incident, rh, m); + if (flip) m->n = c2Neg(m->n); +} + +#endif // CUTE_C2_IMPLEMENTATION_ONCE +#endif // CUTE_C2_IMPLEMENTATION + +/* + ------------------------------------------------------------------------------ + This software is available under 2 licenses - you may choose the one you like. + ------------------------------------------------------------------------------ + ALTERNATIVE A - zlib license + Copyright (c) 2023 Randy Gaul https://randygaul.github.io/ + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from + the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not + be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + ------------------------------------------------------------------------------ + ALTERNATIVE B - Public Domain (www.unlicense.org) + This is free and unencumbered software released into the public domain. + Anyone is free to copy, modify, publish, use, compile, sell, or distribute this + software, either in source code form or as a compiled binary, for any purpose, + commercial or non-commercial, and by any means. + In jurisdictions that recognize copyright laws, the author or authors of this + software dedicate any and all copyright interest in the software to the public + domain. We make this dedication for the benefit of the public at large and to + the detriment of our heirs and successors. We intend this dedication to be an + overt act of relinquishment in perpetuity of all present and future rights to + this software under copyright law. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION + WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + ------------------------------------------------------------------------------ +*/ diff --git a/engine/geometry/cute_c2_ext.h b/engine/geometry/cute_c2_ext.h new file mode 100644 index 0000000..cda2db5 --- /dev/null +++ b/engine/geometry/cute_c2_ext.h @@ -0,0 +1,35 @@ +#include "cute_c2.h" +double PointToSegmentTOI(c2v p, c2v v, c2v a, c2v b); + +double PointToCircleTOI(c2v p, c2v v, c2v circ_p, double r); + +double PolyToPolyTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point); + + +double PolyToCircleTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point); + + + +double CircleToPolyTOI(c2Circle cA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point); + +double CircleToCircleTOI(c2Circle cA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point); + + +double PolyToCapsuleTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point); + +double CapsuleToPolyTOI(c2Capsule cA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point); + +double CapsuleToCapsuleTOI(c2Capsule cA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point); + +double CapsuleToCircleTOI(c2Capsule cA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point); +double CircleToCapsuleTOI(c2Circle cA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point); + +//AABB stuff +double PolyToAABBTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point); +double CircleToAABBTOI(c2Circle cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point); +double CapsuleToAABBTOI(c2Capsule cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point); + +double AABBToPolyTOI(c2AABB bA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point); +double AABBToCircleTOI(c2AABB bA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point); +double AABBToCapsuleTOI(c2AABB bA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point); +double AABBToAABBTOI(c2AABB bA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point); diff --git a/engine/geometry/geometry.c b/engine/geometry/geometry.c new file mode 100644 index 0000000..4fc2a0b --- /dev/null +++ b/engine/geometry/geometry.c @@ -0,0 +1,643 @@ +#define CUTE_C2_IMPLEMENTATION +#include "cute_c2_ext.h" +#include + +double PointToSegmentTOI(c2v p, c2v v, c2v a, c2v b){ + c2v n = c2CCW90(c2Sub(b, a)); //this could be passed in + c2v PA = c2Sub(a, p); + c2v PB = c2Sub(b, p); + + double denom = (c2Dot(v, n)); + if(denom == 0) return INFINITY; //parallel, never collide + double t = (c2Dot(PA, n))/denom; + //if(t<0) return INFINITY; //collided in the past + + if(c2Det2(PA, v)*c2Det2(PB, v) > 0) return INFINITY; //off the sides, never collide + return t; +} + +//adapted from https://stackoverflow.com/a/1084899 +double PointToCircleTOI(c2v p, c2v v, c2v circ_p, double r){ + double a = c2Dot(v, v); + c2v f = c2Sub(p, circ_p); + double b = 2*c2Dot(f, v); + double c = c2Dot(f, f)-r*r; + + double discriminant = b*b-4*a*c; + if(discriminant < 0) return INFINITY; + discriminant = c2Sqrt(discriminant); + + /*double t1 = (-b - discriminant)/(2*a); + double t2 = (-b + discriminant)/(2*a); //this is the one you'd use for exit point TOI + return c2Min(t1, t2);*/ + return (-b - discriminant)/(2*a); +} + +double PolyToPolyTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){ + //return c2TOI(pA, C2_TYPE_POLY, ax_ptr, vA, pB, C2_TYPE_POLY, bx_ptr, vB, true, out_normal, out_contact_point, NULL); + + //degenerate cases, zero movement + if(vA.x==vB.x && vA.y==vB.y) return INFINITY; + + double t = INFINITY; + c2v n = {0,0}; + c2v p = {0,0}; + + //pre-transform poly vertices to avoid doing it every access, compute which edges and faces count as "leading" + c2Poly A = *pA; + c2Poly B = *pB; + + c2v vA2B = c2Sub(vA, vB); + c2v vB2A = c2Sub(vB, vA); + + bool leading_edges_a[C2_MAX_POLYGON_VERTS] = {0}; + bool leading_verts_a[C2_MAX_POLYGON_VERTS] = {0}; + bool leading_edges_b[C2_MAX_POLYGON_VERTS] = {0}; + bool leading_verts_b[C2_MAX_POLYGON_VERTS] = {0}; + + for(int i = 0; ir, A.norms[i]); + if(c2Dot(A.norms[i], vB2A) < 0) leading_edges_a[i] = 1; + } + for(int i = 0; ir, B.norms[i]); + if(c2Dot(B.norms[i], vA2B) < 0) leading_edges_b[i] = 1; + } + for(int i = 0; ir, A.norms[i]); + if(c2Dot(A.norms[i], vB2A) < 0) leading_edges_a[i] = 1; + } + + + //TOI of B against A's vertices + for(int i = 0; ir, A.norms[i]); + if(c2Dot(A.norms[i], vB2A) < 0) leading_edges_a[i] = 1; + } + + bool p_on_cap = true; + + //TOI of B's endcircles against A's vertices + for(int i = 0; i 0){ + leading_capsule_n = c2Neg(leading_capsule_n); + } + + if(d != 0){ + c2v cap_offset = c2Mulvs(leading_capsule_n, cB.r); + + for(int i = 0; i 0){ + leading_edge = c2Neg(leading_edge); + } + bool p_on_a = true; + + if(d != 0){ + c2v cap_offset = c2Mulvs(leading_edge, sum_radius); + + {//a + double v_t = PointToSegmentTOI(cB.a, vB2A, c2Add(cap_offset, cA.a), c2Add(cap_offset, cA.b)); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + p = cB.a; + p_on_a = false; + n = leading_edge; + } + } + + {//b + double v_t = PointToSegmentTOI(cB.b, vB2A, c2Add(cap_offset, cA.a), c2Add(cap_offset, cA.b)); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + p = cB.b; + p_on_a = false; + n = leading_edge; + } + } + } + + + //TOI of circles of A against leading edge of B + leading_edge = c2SafeNorm(c2CCW90(c2Sub(cB.b, cB.a))); + d = c2Dot(leading_edge, vA2B); + if(d > 0){ + leading_edge = c2Neg(leading_edge); + } + if(d != 0){ + c2v cap_offset = c2Mulvs(leading_edge, sum_radius); + + {//a + double v_t = PointToSegmentTOI(cA.a, vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b)); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + p = cA.a; + p_on_a = true; + n = c2Neg(leading_edge); + } + } + + {//a + double v_t = PointToSegmentTOI(cA.b, vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b)); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + p = cA.b; + p_on_a = true; + n = c2Neg(leading_edge); + } + } + } + + + //TOI circles of B against circles of A + + {//aa + double v_t = PointToCircleTOI(cB.a, vB2A, cA.a, sum_radius); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + p = cA.a; + p_on_a = true; + c2v pA = c2Add(c2Mulvs(vA, t), cA.a); + c2v pB = c2Add(c2Mulvs(vB, t), cB.a); + n = c2Sub(pB, pA); + } + } + + {//ba + double v_t = PointToCircleTOI(cB.b, vB2A, cA.a, sum_radius); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + p = cA.a; + p_on_a = true; + c2v pA = c2Add(c2Mulvs(vA, t), cA.a); + c2v pB = c2Add(c2Mulvs(vB, t), cB.b); + n = c2Sub(pB, pA); + } + } + {//ab + double v_t = PointToCircleTOI(cB.a, vB2A, cA.b, sum_radius); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + p = cA.b; + p_on_a = true; + c2v pA = c2Add(c2Mulvs(vA, t), cA.b); + c2v pB = c2Add(c2Mulvs(vB, t), cB.a); + n = c2Sub(pB, pA); + } + } + + {//bb + double v_t = PointToCircleTOI(cB.b, vB2A, cA.b, sum_radius); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + p = cA.b; + p_on_a = true; + c2v pA = c2Add(c2Mulvs(vA, t), cA.b); + c2v pB = c2Add(c2Mulvs(vB, t), cB.b); + n = c2Sub(pB, pA); + } + } + + + n = c2SafeNorm(n); + if(out_normal) *out_normal = n; + if(out_contact_point) { + if(p_on_a){ + *out_contact_point = c2Add(p, c2Add(c2Mulvs(n, cA.r), c2Mulvs(vA, t))); + } else { + *out_contact_point = c2Add(p, c2Add(c2Mulvs(c2Neg(n), cB.r), c2Mulvs(vB, t))); + } + } + return t; +} + + +double CapsuleToCircleTOI(c2Capsule cA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){ + double t = CircleToCapsuleTOI(cB, vB, cA, vA, out_normal, out_contact_point); + if(out_normal) *out_normal = c2Neg(*out_normal); + return t; +} +double CircleToCapsuleTOI(c2Circle cA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){ + //return c2TOI(&cA, C2_TYPE_CIRCLE, NULL, vA, &cB, C2_TYPE_CAPSULE, NULL, vB, true, out_normal, out_contact_point, NULL); + + //degenerate cases, zero movement + if(vA.x==vB.x && vA.y==vB.y) return INFINITY; + + double t = INFINITY; + c2v n = {0,0}; + c2v p = {0,0}; + + c2v vA2B = c2Sub(vA, vB); + c2v vB2A = c2Sub(vB, vA); + + double sum_radius = cA.r+cB.r; + + + + //TOI of A against leading edge of B + c2v leading_edge = c2SafeNorm(c2CCW90(c2Sub(cB.b, cB.a))); + double d = c2Dot(leading_edge, vA2B); + if(d > 0){ + leading_edge = c2Neg(leading_edge); + } + if(d != 0){ + c2v cap_offset = c2Mulvs(leading_edge, sum_radius); + + double v_t = PointToSegmentTOI(cA.p, vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b)); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + n = c2Neg(leading_edge); + } + } + + //TOI of A against circles of B + + {//a + double v_t = PointToCircleTOI(cA.p, vA2B, cB.a, sum_radius); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + c2v pA = c2Add(c2Mulvs(vA, t), cA.p); + c2v pB = c2Add(c2Mulvs(vB, t), cB.a); + n = c2Sub(pB, pA); + } + } + + {//b + double v_t = PointToCircleTOI(cA.p, vA2B, cB.b, sum_radius); + if(v_t <= t){ + if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case + + t = v_t; + c2v pA = c2Add(c2Mulvs(vA, t), cA.p); + c2v pB = c2Add(c2Mulvs(vB, t), cB.b); + n = c2Sub(pB, pA); + } + } + + + + n = c2SafeNorm(n); + if(out_normal) *out_normal = n; + if(out_contact_point) *out_contact_point = c2Add(cA.p, c2Add(c2Mulvs(n, cA.r), c2Mulvs(vA, t))); + return t; + +} + + + +//AABB stuff +double PolyToAABBTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){ + c2Poly pB; + c2BBVerts(pB.verts, &bB); + pB.count = 4; + c2Norms(pB.verts, pB.norms, 4); + + return PolyToPolyTOI(pA, ax_ptr, vA, &pB, NULL, vB, out_normal, out_contact_point); +} +double CircleToAABBTOI(c2Circle cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){ + c2Poly pB; + c2BBVerts(pB.verts, &bB); + pB.count = 4; + c2Norms(pB.verts, pB.norms, 4); + + return CircleToPolyTOI(cA, vA, &pB, NULL, vB, out_normal, out_contact_point); +} +double CapsuleToAABBTOI(c2Capsule cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){ + c2Poly pB; + c2BBVerts(pB.verts, &bB); + pB.count = 4; + c2Norms(pB.verts, pB.norms, 4); + + return CapsuleToPolyTOI(cA, vA, &pB, NULL, vB, out_normal, out_contact_point); +} + +double AABBToPolyTOI(c2AABB bA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){ + c2Poly pA; + c2BBVerts(pA.verts, &bA); + pA.count = 4; + c2Norms(pA.verts, pA.norms, 4); + + return PolyToPolyTOI(&pA, NULL, vA, pB, bx_ptr, vB, out_normal, out_contact_point); +} +double AABBToCircleTOI(c2AABB bA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){ + c2Poly pA; + c2BBVerts(pA.verts, &bA); + pA.count = 4; + c2Norms(pA.verts, pA.norms, 4); + + return PolyToCircleTOI(&pA, NULL, vA, cB, vB, out_normal, out_contact_point); +} +double AABBToCapsuleTOI(c2AABB bA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){ + c2Poly pA; + c2BBVerts(pA.verts, &bA); + pA.count = 4; + c2Norms(pA.verts, pA.norms, 4); + + return PolyToCapsuleTOI(&pA, NULL, vA, cB, vB, out_normal, out_contact_point); +} +double AABBToAABBTOI(c2AABB bA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){ + c2Poly pA; + c2BBVerts(pA.verts, &bA); + pA.count = 4; + c2Norms(pA.verts, pA.norms, 4); + + c2Poly pB; + c2BBVerts(pB.verts, &bB); + pB.count = 4; + c2Norms(pB.verts, pB.norms, 4); + + return PolyToPolyTOI(&pA, NULL, vA, &pB, NULL, vB, out_normal, out_contact_point); +} diff --git a/engine/tests/CMakeLists.txt b/engine/tests/CMakeLists.txt index 1ad6a55..648a099 100644 --- a/engine/tests/CMakeLists.txt +++ b/engine/tests/CMakeLists.txt @@ -1 +1,2 @@ add_subdirectory(base) +add_subdirectory(geometry) diff --git a/engine/tests/geometry/CMakeLists.txt b/engine/tests/geometry/CMakeLists.txt new file mode 100644 index 0000000..a4b4bc0 --- /dev/null +++ b/engine/tests/geometry/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(samples) diff --git a/engine/tests/geometry/samples/CMakeLists.txt b/engine/tests/geometry/samples/CMakeLists.txt new file mode 100644 index 0000000..0459c92 --- /dev/null +++ b/engine/tests/geometry/samples/CMakeLists.txt @@ -0,0 +1,8 @@ +add_executable(collisionSample collision_sample.c) +target_link_libraries(collisionSample PRIVATE + engine +) +set_target_properties(collisionSample + PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin/manual) + diff --git a/engine/tests/geometry/samples/collision_sample.c b/engine/tests/geometry/samples/collision_sample.c new file mode 100644 index 0000000..d4b8dd9 --- /dev/null +++ b/engine/tests/geometry/samples/collision_sample.c @@ -0,0 +1,209 @@ +#include "cute_c2_ext.h" +#include "raylib.h" +#include "raymath.h" + +#include + +const int screenWidth = 800; +const int screenHeight = 450; + +typedef struct Shape { + Vector2 pos; + Vector2 vel; + C2_TYPE type; + union { + c2v boxDim; + c2Circle circle; + c2Poly poly; + } shape; + Color colour; +} Shape; + + +void DrawShape(const Shape* shape) { + switch (shape->type) + { + case C2_TYPE_AABB: + DrawRectangle( + shape->pos.x - shape->shape.boxDim.x / 2, + shape->pos.y - shape->shape.boxDim.y / 2, + shape->shape.boxDim.x, + shape->shape.boxDim.y, + shape->colour + ); + break; + case C2_TYPE_CIRCLE: + DrawCircle( + shape->pos.x, + shape->pos.y, shape->shape.circle.r, + shape->colour + ); + break; + case C2_TYPE_POLY: + for (int i = 0; i < shape->shape.poly.count; ++i) { + int next = (i + 1 + shape->shape.poly.count) % shape->shape.poly.count; + DrawLineV( + (Vector2){shape->shape.poly.verts[i].x, shape->shape.poly.verts[i].y}, + (Vector2){shape->shape.poly.verts[next].x, shape->shape.poly.verts[next].y}, + shape->colour + ); + } + break; + default: + break; + } +} + +struct TOIInfo { + double toi; + c2v contact; + c2v normal; +}; + +void draw_toi_info(const struct TOIInfo* info) +{ + if (info->toi != INFINITY) + { + DrawCircle(info->contact.x, info->contact.y, 4, GREEN); + DrawLineEx( + (Vector2){info->contact.x, info->contact.y}, + (Vector2){ + info->contact.x + info->normal.x * 12, + info->contact.y + info->normal.y * 12 + }, + 2, GREEN + ); + } +} + +int main(void) +{ + InitWindow(screenWidth, screenHeight, "raylib"); + SetTargetFPS(60); + + Shape A = { + .pos = {300,150}, + .type = C2_TYPE_CIRCLE, + //.shape.box = {{0, 0}, {64, 64}}, + .shape.circle = {{0, 0}, 64}, + .colour = RED, + }; + + Shape B = { + .pos = {43,60}, + .type = C2_TYPE_AABB, + .shape.boxDim = {32, 32}, + .colour = BLUE, + }; + + Shape C = { + .pos = {100,100}, + .type = C2_TYPE_AABB, + .shape.boxDim = {64, 64}, + .colour = PURPLE, + }; + + Shape D = { + .pos = {200,300}, + .type = C2_TYPE_POLY, + .shape.poly = { + .count = 3, + .verts = { + {0,0}, + {100, 0}, + {50,50}, + }, + }, + .colour = RED, + }; + c2MakePoly(&D.shape.poly); + for (int i = 0; i < D.shape.poly.count; ++i) { + D.shape.poly.verts[i].x += D.pos.x; + D.shape.poly.verts[i].y += D.pos.y; + } + + while (!WindowShouldClose()) + { + float frame_time = GetFrameTime(); + Vector2 move_dir = {0}; + + if (IsKeyDown(KEY_LEFT)) { + move_dir.x += -1; + } + if (IsKeyDown(KEY_RIGHT)) { + move_dir.x += 1; + } + if (IsKeyDown(KEY_UP)) { + move_dir.y += -1; + } + if (IsKeyDown(KEY_DOWN)) { + move_dir.y += 1; + } + + move_dir = Vector2Normalize(move_dir); + B.vel = Vector2Scale(move_dir, 100); + + B.pos = Vector2Add( + B.pos , Vector2Scale(B.vel, frame_time) + ); + + c2Circle Acirc= { + {A.pos.x, A.pos.y}, + A.shape.circle.r + }; + c2AABB Bbox= { + {B.pos.x - B.shape.boxDim.x / 2, B.pos.y - B.shape.boxDim.y / 2}, + {B.pos.x + B.shape.boxDim.x / 2, B.pos.y + B.shape.boxDim.y / 2}, + }; + c2AABB Cbox= { + {C.pos.x - C.shape.boxDim.x / 2, C.pos.y - C.shape.boxDim.y / 2}, + {C.pos.x + C.shape.boxDim.x / 2, C.pos.y + C.shape.boxDim.y / 2}, + }; + + Vector2 test_spd = Vector2Normalize(Vector2Subtract(C.pos, B.pos)); + c2v test_velocityB = {test_spd.x * 100, test_spd.y * 100}; + const c2v static_velocity = {0,0}; + + struct TOIInfo toi0 = {0}; + toi0.toi = AABBToAABBTOI( + Bbox, test_velocityB, + Cbox, static_velocity, + &toi0.normal, &toi0.contact + ); + + test_spd = Vector2Normalize(Vector2Subtract(A.pos, B.pos)); + test_velocityB = (c2v){test_spd.x * 100, test_spd.y * 100}; + struct TOIInfo toi1 = {0}; + toi1.toi = AABBToCircleTOI( + Bbox, test_velocityB, + Acirc, static_velocity, + &toi1.normal, &toi1.contact + ); + + test_spd = Vector2Normalize(Vector2Subtract(D.pos, B.pos)); + test_velocityB = (c2v){test_spd.x * 100, test_spd.y * 100}; + struct TOIInfo toi2 = {0}; + toi2.toi = AABBToPolyTOI( + Bbox, test_velocityB, + &D.shape.poly, NULL, static_velocity, + &toi2.normal, &toi2.contact + ); + + //char buf[32]; + BeginDrawing(); + ClearBackground(RAYWHITE); + DrawShape(&A); + DrawShape(&C); + DrawShape(&D); + DrawShape(&B); + DrawLineEx(B.pos, A.pos, 1, BLACK); + DrawLineEx(C.pos, A.pos, 1, BLACK); + DrawLineEx(D.pos, A.pos, 1, BLACK); + //DrawText(buf, A.pos.x, A.pos.y, 12, BLACK); + draw_toi_info(&toi0); + draw_toi_info(&toi1); + draw_toi_info(&toi2); + EndDrawing(); + } + return 0; +}