diff --git a/.gitignore b/.gitignore index 701ae90..c90da6b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ physics.exe +checker.exe *.pdb +*.obj .build/ +.vs/ diff --git a/build_checker.bat b/build_checker.bat new file mode 100644 index 0000000..a692d7c --- /dev/null +++ b/build_checker.bat @@ -0,0 +1 @@ +cl /Zi /FS /Fd"checker.pdb" checker.c diff --git a/checker.c b/checker.c new file mode 100644 index 0000000..6dbaf7e --- /dev/null +++ b/checker.c @@ -0,0 +1,68 @@ +#include +#define CUTE_C2_IMPLEMENTATION +#include "cute_c2.h" + +int main(int argc, char ** argv) +{ + int face_index = -1; + #define V(xin, yin) (c2v){.x = xin, .y = yin} + #define VN(x, y) (c2Norm(V(x, y))) +#if 0 + c2v vert = V(1.0, 2.0); + c2v vert_no_macro = (c2v){.x = 1.0, .y = 2.0}; +#endif +#if 0 + c2Poly A = (c2Poly){ + .count = 1, + .verts = {V(-1.0, 26.0)}, + .norms = {V(-1.0, 0.0)}, + }; + printf("%f\n", A.verts[0].y); +#endif + +#if 1 + c2Poly A = (c2Poly){ + .count = 4, + .verts = {V(-1, 1), V(1, 1), V(1, -1), V(-1, -1)}, + }; + c2MakePoly(&A); + c2Poly B = (c2Poly){ + .count = 4, + .verts = {V(-0.5, 1), V(1.5, 1), V(1.5, -1), V(-0.5, -1)}, + }; + c2MakePoly(&B); + + c2v b_offset = V(0.0, 0.0); + for(int i = 0; i < B.count; i++) { + B.verts[i].x += b_offset.x; + B.verts[i].y += b_offset.y; + } + + printf("A\n"); + for(int i = 0; i < A.count; i++) { + printf("(%f,%f), ", A.verts[i].x, A.verts[i].y); + } + printf("\n"); + printf("Normals: "); + for(int i = 0; i < A.count; i++) { + printf("(%f,%f), ", A.norms[i].x, A.norms[i].y); + } + printf("\n"); + printf("\n"); + printf("B\n"); + for(int i = 0; i < B.count; i++) { + printf("(%f,%f), ", B.verts[i].x, B.verts[i].y); + } + printf("\n"); + + float output = c2CheckFaces(&A, c2xIdentity(), &B, c2xIdentity(), &face_index); + + printf("Separation %f face_index %d\n", output, face_index); + + c2Manifold m = {0}; + c2PolytoPolyManifold(&A, NULL, &B, NULL, &m); + printf("Manifold normal (%f, %f) | depth 0 %f | depth 1 %f |\n", m.n.x, m.n.y, m.depths[0], m.depths[1]); + +#endif + getchar(); // so terminal doesn't close immediately in remedybg +} diff --git a/cute_c2.h b/cute_c2.h new file mode 100644 index 0000000..5ac395c --- /dev/null +++ b/cute_c2.h @@ -0,0 +1,2265 @@ +/* + ------------------------------------------------------------------------------ + 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_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; + + while (result.iterations < 20 && c2Len(v) - radius > tolerance) + { + 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) { + 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: http://www.randygaul.net/2014/07/23/distance-point-to-line-segment/ +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) 2017 Randy Gaul http://www.randygaul.net + 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/debug.rdbg b/debug.rdbg new file mode 100644 index 0000000..f5fcb01 Binary files /dev/null and b/debug.rdbg differ diff --git a/physics.jai b/physics.jai index 1c61484..260781e 100644 --- a/physics.jai +++ b/physics.jai @@ -1,6 +1,7 @@ #import "Basic"; #import "Math"; #import "Window_Creation"; +Debug :: #import "Debug"; Input :: #import "Input"; Simp :: #import "Simp"; @@ -11,11 +12,32 @@ window_height : s32 = 720; xy :: (x: int, y: int) -> Vector2 { return xy(cast(float)x, cast(float)y); } +normalize :: (using v: *Vector2) -> float { + sq := sqrt(x*x + y*y); + + factor := 1.0 / sq; + x *= factor; + y *= factor; + + return sq; +} +normalize :: (v: Vector2) -> Vector2 #must { + normalize(*v); + return v; +} +negative :: (v: Vector2) -> Vector2 #must { + return xy(-v.x, -v.y); +} drawing_in_world_space: bool = false; WHITE :: Vector4.{1.0, 1.0, 1.0, 1.0}; +RED :: Vector4.{1.0, 0.0, 0.0, 1.0}; GREEN :: Vector4.{0.0, 1.0, 0.0, 1.0}; drawing_color: Vector4 = WHITE; line_thickness: float = 1.0; +defaults :: () { + drawing_color = WHITE; + line_thickness = 1.0; +} line :: (_from: Vector2, _to: Vector2) { width := line_thickness; from := _from; @@ -24,33 +46,100 @@ line :: (_from: Vector2, _to: Vector2) { from = world_to_screen(from); to = world_to_screen(to); } - Simp.set_shader_for_color(); + Simp.set_shader_for_color(true); normal := rotate(unit_vector(to - from), PI/2.0); Simp.immediate_quad(from + normal*width, from - normal*width, to + normal*width, to - normal*width, color = drawing_color); } +LastingPip :: struct { + alive_for: float; + pos: Vector2; +} +pips : [10] LastingPip; +push_pip :: (at: Vector2) +{ + Debug.breakpoint(); + for * pips + { + if it.alive_for <= 0.0 + { + it.alive_for = 1.0; + it.pos = at; + break; + } + } +} +draw_pips :: (dt: float) +{ + drawing_in_world_space = true; + drawing_color = RED; + for * pips if it.alive_for > 0.0 + { + drawing_color.w = it.alive_for; + pip(it.pos); + it.alive_for -= dt; + } + defaults(); +} +pip :: (_at: Vector2, size: float = 0.05) { + at := _at; + Simp.set_shader_for_color(true); + if drawing_in_world_space { + at = world_to_screen(at); + size /= camera.zoom; + } + Simp.immediate_quad(at + xy(-size, size), at + xy(size, size), at + xy(size, -size), at + xy(-size, -size), color = drawing_color); +} Rect :: struct { halfsize: Vector2; pos , vel , force : Vector2; // pos is in world space angle, angle_vel, torque: float; + mass: float = 1.0; }; -MASS :: 1.0; moment_of_inertia :: (using r: Rect) -> float { - return MASS*(halfsize.y*halfsize.y + halfsize.x*halfsize.x)/12.0; + return mass*(halfsize.y*halfsize.y + halfsize.x*halfsize.x)/12.0; } apply_force_at_point :: (r: *Rect, force: Vector2, point_world_space: Vector2) { r.force += force; offset_from_center_of_mass := point_world_space - r.pos; r.torque += offset_from_center_of_mass.x * force.y - offset_from_center_of_mass.y * force.x; } +// everything needed to resolve the collision Manifold :: struct { count: int; depths: [2] float; - contact_points: [2] Vector2; - normal: Vector2; + contact_points: [2] Vector2; // in absolute coordinates + normal: Vector2; // always points from shape A to shape B }; +cross :: (v1: Vector2, v2: Vector2) -> float { + return v1.x*v2.y - v1.y*v2.x; +} +/// Returns a perpendicular vector. (90 degree rotation) +perp :: (v: Vector2) -> Vector2 +{ + return xy(-v.y, v.x); +} +handle_collision :: (m: Manifold, a: Rect, b: Rect) { + if m.count == 0 return; + k_scalar :: (a: Rectangle, b: Rectangle, r1: Vector2, r2: Vector2, n: Vector2) { + k_scalar_rect :: (a: Rectangle, r: Vector2, n: Vector2) + { + rcn := cross(r, n); + return 1.0/a.mass + rcn*rcn / moment_of_inertia(a); + } + + cpFloat value = k_scalar_rect(a, r1, n) + k_scalar_rect(b, r2, n); + assert(value != 0.0, "Unsolvable collision or constraint."); + return value; + } + r1 := m.contact_points[0] - a.pos; + r2 := m.contact_points[0] - b.pos; + nMass := 1.0 / k_scalar(a, b, r1, r2, + +} MAX_POLYGON_VERTS :: 8; Polygon :: struct { + // verts.count needs to equal norms.count count: int; verts: [MAX_POLYGON_VERTS] Vector2; norms: [MAX_POLYGON_VERTS] Vector2; @@ -60,49 +149,281 @@ Halfspace :: struct { n: Vector2; d: float; } +CCW90 :: (a: Vector2) -> Vector2 { + b: Vector2; + b.x = a.y; + b.y = -a.x; + return b; +} +distance :: (h: Halfspace, p: Vector2) -> float { + return dot(h.n, p) - h.d; +} +intersect :: (a: Vector2, b: Vector2, da: float, db: float) -> Vector2 { + return a + (b - a) * (da / (da - db)); +} poly_from :: (using r: Rect) -> Polygon { to_return: Polygon; + compute_normals :: (p: *Polygon) { + for 0..p.count-1 { + a: int = it; + b: int = ifx it + 1 < p.count then it + 1 else 0; + e: Vector2 = p.verts[b] - p.verts[a]; + p.norms[it] = normalize(CCW90(e)); + } + } + facing_to_right := rotate(xy(halfsize.x,0.0), angle); facing_to_up := rotate(xy(0.0,halfsize.y), angle); to_return.count = 4; - to_return.verts[0] = -facing_to_right + facing_up; // upper left - to_return.norms[0] = #run normalize(xy(-1.0, 1.0)); - to_return.verts[1] = facing_to_right + facing_up; // upper right - to_return.norms[1] = #run normalize(xy(1.0, 1.0)); + // the ordering here is important for collision algorithms, not exactly sure why. + // Perhaps it's just important that the winding is counter clockwise + to_return.verts[0] = facing_to_right + -facing_to_up; // lower right + to_return.verts[1] = facing_to_right + facing_to_up; // upper right + to_return.verts[2] = -facing_to_right + facing_to_up; // upper left + to_return.verts[3] = -facing_to_right + -facing_to_up; // lower left - to_return.verts[2] = facing_to_right + -facing_up; // lower right - to_return.norms[2] = #run normalize(xy(1.0, -1.0)); - - to_return.verts[3] = -facing_to_right + -facing_up; // lower left - to_return.norms[3] = #run normalize(xy(-1.0, -1.0)); + //for 0..to_return.count-1 to_return.norms[it] = rotate(to_return.norms[it], angle); + for 0..to_return.count-1 to_return.verts[it] += pos; + compute_normals(*to_return); return to_return; } -rect_to_rect :: (a: Rect, b: Rect) -> Manifold { - to_return : Manifold; - check_faces :: (a: Polygon, b: Polygon) -> float { +check_faces :: (a: Polygon, b: Polygon) -> (separation: float, face_index: int) { - plane_at :: (p : Polygon, i: int) -> Halfspace { - return .{n = p.norms[i], d = dot(p.norms[i], p.verts[i]}; - } + plane_at :: (p : Polygon, i: int) -> Halfspace { + return .{n = p.norms[i], d = dot(p.norms[i], p.verts[i])}; + } - support :: (verts: [] Vector2, d: Vector2 { + support :: (verts: [] Vector2, d: Vector2) -> int { + imax: int = 0; + dmax: float = dot(verts[0], d); + for 1..verts.count-1 { + dot_output: float = dot(verts[it], d); + if(dot_output > dmax) + { + imax = it; + dmax = dot_output; + } + } + return imax; + } + sep : float = -FLOAT32_MAX; + index: int = ~0; + + for 0..a.count-1 { + h := plane_at(a, it); + verts_to_use: [] Vector2 = b.verts; + verts_to_use.count = b.count; + idx := support(verts_to_use, negative(h.n)); + p := b.verts[idx]; + d := distance(h, p); + if (d > sep) + { + sep = d; + index = it; } + } + + return sep, index; +} + +rect_to_rect :: (a: Rect, b: Rect) -> Manifold { + to_return : Manifold; + + poly_a := poly_from(a); + poly_b := poly_from(b); + + sa, ea := check_faces(poly_a, poly_b); + + if sa >= 0 return .{}; + sb, eb := check_faces(poly_b, poly_a); + if sb >= 0 return .{}; + + rp, ip: *Polygon; + re: int; + flip: int; + kRelTol := 0.95; + kAbsTol := 0.01; + + if(sa * kRelTol > sb + kAbsTol) + { + rp = *poly_a; + ip = *poly_b; + re = ea; + flip = 0; + } + else + { + rp = *poly_b; + ip = *poly_a; + re = eb; + flip = 1; + } + incident: [2] Vector2; + // calculate incident + { + rn_in_incident_space := rp.norms[re]; - sep : float = FLT_MAX; index: int = ~0; + min_dot: float = FLOAT32_MAX; + for 0..ip.count-1 { + dot_output := dot(rn_in_incident_space, ip.norms[it]); + if(dot_output < min_dot) + { + min_dot = dot_output; + index = it; + } + } + incident[0] = ip.verts[index]; + incident[1] = ip.verts[ifx index + 1 == ip.count then 0 else index + 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 + // side planes from poly + rh: Halfspace; + side_planes_poly_return: int; + { + seg: [2] Vector2 = incident; + p: *Polygon = rp; + e: int = re; + + ra := p.verts[e]; + rb := p.verts[ifx e + 1 == p.count then 0 else e + 1]; + + // side planes + side_planes :: (seg: [] Vector2, ra: Vector2, rb: Vector2) -> int, Halfspace { + in: Vector2 = normalize(rb - ra); + left: Halfspace = .{ n = negative(in), d = dot(negative(in), ra) }; + right: Halfspace = .{ n = in, d = dot(in, rb) }; + + // clip a segment to a plane + clip :: (seg: [] Vector2, h: Halfspace) -> int { + out: [2] Vector2; + sp: int = 0; + d0: float = distance(h, seg[0]); + d1: float = distance(h, seg[1]); + if d0 < 0 { out[sp] = seg[0]; sp+=1; } + if d1 < 0 { out[sp] = seg[1]; sp+=1; } + if d0 == 0 && d1 == 0 { + out[sp] = seg[0]; + sp+=1; + out[sp] = seg[1]; + sp+=1; + } else if d0 * d1 <= 0 { + out[sp] = intersect(seg[0], seg[1], d0, d1); + sp+=1; + } + seg[0] = out[0]; + seg[1] = out[1]; + return sp; + } + + if clip(seg, left) < 2 return 0, .{}; + if clip(seg, right) < 2 return 0, .{}; - for 0..a.count-1 { - h := plane_at(a, i); - idx := + h: Halfspace; + h.n = CCW90(in); + h.d = dot(CCW90(in), ra); + + return 1, h; + } + side_planes_poly_return, rh = side_planes(seg, ra, rb); + } + if !side_planes_poly_return return .{}; + + m: Manifold; + // keep deep + { + seg: [2] Vector2 = incident; + h: Halfspace = rh; + cp: int = 0; + for 0..1 { + p := seg[it]; + d: float = distance(h, p); + if (d <= 0) + { + m.contact_points[cp] = p; + m.depths[cp] = -d; + cp+=1; + } } + m.count = cp; + m.normal = h.n; } + if flip m.normal = negative(m.normal); + for 0..m.count-1 { + push_pip(m.contact_points[it]); + } + return m; +} +dump_polygon :: (poly: Polygon) { + for poly.verts print("V(%, %), ", it.x, it.y); + print("\n"); + for poly.norms print("VN(%, %), ", it.x, it.y); + print("\n"); +} + +do_test :: () { + print("Doing test\n"); + size := xy(1.0, 1.0); + A_rect: Rect = .{halfsize = size}; + B_rect: Rect = .{halfsize = size, pos = xy(0.5, 0.0)}; +#if true { + print("A %\n", A_rect); + print("B %\n", B_rect); + print("Collision %\n", rect_to_rect(A_rect, B_rect)); +} + + +#if false { + A: Polygon; + B: Polygon; + + A = poly_from(A_rect); + B = poly_from(B_rect); + + print("A\n"); + dump_polygon(A); + print("B\n"); + dump_polygon(B); + + /*for A.verts print("V(%,%), ", it.x, it.y); + print("\n"); + for A.norms print("VN(%,%), ", it.x, it.y); + print("\n");*/ + + /*A.count = 3; + A.verts[0] = xy(-1.0, 0.0); + A.verts[1] = xy(-1.0, 1.0); + A.verts[2] = xy(0.0, 0.0); + A.norms[0] = #run normalize(xy(-1.0, 0.0)); + A.norms[1] = #run normalize(xy(0.0, 1.0)); + A.norms[2] = #run normalize(xy(1.0, 0.0)); + */ + + /*B.count = 3; + B.verts[0] = xy(2.0, 0.0); + B.verts[1] = xy(2.0, 1.0); + B.verts[2] = xy(1.0, 0.0); + B.norms[0] = #run normalize(xy(1.0, 0.0)); + B.norms[1] = #run normalize(xy(0.0, 1.0)); + B.norms[2] = #run normalize(xy(-1.0, 0.0)); + */ + + //for 0..B.count-1 B.verts[it] += xy(-0.5, 0.0); + + seperation, face_index := check_faces(A, B); + print("Separation % face_index %\n", seperation, face_index); +} + Debug.breakpoint(); } draw_rect :: (using r: Rect) { @@ -149,6 +470,12 @@ world_to_screen :: (world: Vector2) -> Vector2 { } main :: () { + #if false + { + do_test(); + return; + } + window := create_window(window_width, window_height, "A Window"); // Actual render size in pixels can be different from the window dimensions we specified above (for example on high-resolution displays on macOS/iOS). window_width, window_height = Simp.get_render_dimensions(window); @@ -205,7 +532,6 @@ main :: () { { defer unprocessed_time -= TIMESTEP; - if get_time() < 0.5 { apply_force_at_point(*rects[0], xy(3, 0), rects[0].pos + xy(-0.1, 0.03)); @@ -216,6 +542,18 @@ main :: () { it.vel += (it.force/MASS) * TIMESTEP; it.pos += it.vel * TIMESTEP; + from_rect := it; + for * to_rect: rects { + if to_rect != from_rect + { + manifold_out := rect_to_rect(from_rect, to_rect); + if manifold_out.count > 0 { + handle_collision(manifold_out, from_rect, to_rect); + //from_rect.vel *= -1.0; + } + } + } + it.angle_vel += (it.torque / moment_of_inertia(it)) * TIMESTEP; it.angle += it.angle_vel * TIMESTEP; } @@ -223,28 +561,31 @@ main :: () { } + + Simp.immediate_begin(); Simp.clear_render_target(0.0, 0.0, 0.0, 1.0); - - drawing_in_world_space = true; // draw grid - drawing_color = .{0.2, 0.2, 0.2, 0.2}; + drawing_in_world_space = true; + drawing_color = .{0.2, 0.2, 0.2, 0.5}; for x: -30..30 { line(xy(x, 30), xy(x, -30)); } for y: -30..30 { line(xy(30, y), xy(-30, y)); } - drawing_color = WHITE; + defaults(); + draw_pips(dt); + drawing_in_world_space = true; line_thickness = 2.0; drawing_color = GREEN; for rects draw_rect(it); line_thickness = 1.0; - drawing_color = WHITE; - //line(xy(0.0, 0.0), screen_to_world(mouse())); - drawing_in_world_space = false; + defaults(); + + Simp.immediate_flush(); Simp.swap_buffers(window);