From 31ddb92d7bd004bfb3c256c4aa7bae16a92570cd Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 15 Mar 2022 14:04:01 +0100 Subject: [PATCH] Upgrade cute lib --- incl/cute_c2.h | 709 ++++++++++++++++++++++++++++++++++--------------- 1 file changed, 501 insertions(+), 208 deletions(-) diff --git a/incl/cute_c2.h b/incl/cute_c2.h index 79848cc..5257b75 100644 --- a/incl/cute_c2.h +++ b/incl/cute_c2.h @@ -9,7 +9,7 @@ Licensing information can be found at the end of the file. ------------------------------------------------------------------------------ - cute_c2.h - v1.06 + cute_c2.h - v1.10 To create implementation (the function definitions) #define CUTE_C2_IMPLEMENTATION @@ -43,10 +43,8 @@ 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 + * 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 @@ -80,28 +78,6 @@ https://github.com/sro5h/tinyc2-tests - DETAILS/ADVICE - - 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. - - Please email at my address with any questions or comments at: - author's last name followed by 1748 at gmail - - FEATURES * Circles, capsules, AABBs, rays and convex polygons are supported @@ -125,10 +101,16 @@ 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 @@ -136,6 +118,52 @@ 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) @@ -146,24 +174,25 @@ // 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 12 +#define C2_MAX_POLYGON_VERTS 8 // 2d vector -typedef struct +typedef struct c2v { float x; float y; } c2v; -// 2d rotation composed of cos/sin pair -typedef struct +// 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 +typedef struct c2m { c2v x; c2v y; @@ -176,40 +205,40 @@ typedef struct // 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 +typedef struct c2x { c2v p; c2r r; } c2x; // 2d halfspace (aka plane, aka line) -typedef struct +typedef struct c2h { c2v n; // normal, normalized float d; // distance to origin from plane, or ax + by = d } c2h; -typedef struct +typedef struct c2Circle { c2v p; float r; } c2Circle; -typedef struct +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 +typedef struct c2Capsule { c2v a; c2v b; float r; } c2Capsule; -typedef struct +typedef struct c2Poly { int count; c2v verts[C2_MAX_POLYGON_VERTS]; @@ -221,14 +250,14 @@ typedef struct // 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 +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 +typedef struct c2Raycast { float t; // time of impact c2v n; // normal of surface at impact (unit length) @@ -239,14 +268,8 @@ typedef struct // 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. cute_c2 does not include -// "feature information" that describes which topological features collided. -// However, modifying the exist ***Manifold funcs can be done to output any -// needed feature information. Feature info is sometimes needed for certain kinds -// of simulations that cache information over multiple game-ticks, of which are -// associated to the collision of specific features. An example implementation -// is in the qu3e 3D physics engine library: https://github.com/RandyGaul/qu3e -typedef struct +// the resolution step is *not* included in cute_c2. +typedef struct c2Manifold { int count; float depths[2]; @@ -265,8 +288,7 @@ typedef struct #endif // boolean collision detection -// these versions are faster than the manifold versions, but only give a YES/NO -// result +// 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); @@ -288,10 +310,10 @@ 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 slower than the boolean versions, but will compute one -// or two points that represent the plane of contact. This information is -// is usually needed to resolve and prevent shapes from colliding. If no coll -// ision occured the count member of the manifold struct is set to 0. +// 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); @@ -314,7 +336,7 @@ typedef enum // This struct is only for advanced usage of the c2GJK function. See comments inside of the // c2GJK function for more details. -typedef struct +typedef struct c2GJKCache { float metric; int count; @@ -323,6 +345,8 @@ typedef struct 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. @@ -330,20 +354,69 @@ typedef struct // 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`. +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. +}; + +// 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. The out_normal for non-colliding configurations (or in -// other words, when the return value is 1) is just the direction pointing along the closest points -// from shape A to shape B. out_normal can be NULL. iterations is an optional parameter. use_radius +// 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). -CUTE_C2_API float c2TOI(const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, int use_radius, c2v* out_normal, c2v* out_contact_point, int* iterations); +// +// 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. @@ -356,7 +429,7 @@ 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, or be NULL for identity transforms. +// 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); @@ -422,7 +495,7 @@ C2_INLINE int c2Parallel(c2v a, c2v b, float kTol) // rotation ops C2_INLINE c2r c2Rot(float radians) { c2r r; c2SinCos(radians, &r.s, &r.c); return r; } -C2_INLINE c2r c2RotIdentity() { c2r r; r.c = 1.0f; r.s = 0; 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); } @@ -436,7 +509,7 @@ C2_INLINE c2m c2Mulmm(c2m a, c2m b) { c2m c; c.x = c2Mulmv(a, b.x); c.y = c2Mu 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() { c2x x; x.p = c2V(0, 0); x.r = c2RotIdentity(); return x; } +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; } @@ -477,7 +550,7 @@ int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const 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; + default: return 0; } break; @@ -488,7 +561,7 @@ int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const 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; + default: return 0; } break; @@ -499,7 +572,7 @@ int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const 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; + default: return 0; } break; @@ -510,7 +583,7 @@ int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const 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; + default: return 0; } break; @@ -674,7 +747,6 @@ static C2_INLINE c2v c2L(c2Simplex* s) { case 1: return s->a.p; case 2: return C2_BARY2(p); - case 3: return C2_BARY3(p); default: return c2V(0, 0); } } @@ -711,8 +783,8 @@ static C2_INLINE void c22(c2Simplex* s) { c2v a = s->a.p; c2v b = s->b.p; - float u = c2Dot(b, c2Norm(c2Sub(b, a))); - float v = c2Dot(a, c2Norm(c2Sub(a, b))); + float u = c2Dot(b, c2Sub(b, a)); + float v = c2Dot(a, c2Sub(a, b)); if (v <= 0) { @@ -744,13 +816,13 @@ static C2_INLINE void c23(c2Simplex* s) c2v b = s->b.p; c2v c = s->c.p; - float uAB = c2Dot(b, c2Norm(c2Sub(b, a))); - float vAB = c2Dot(a, c2Norm(c2Sub(a, b))); - float uBC = c2Dot(c, c2Norm(c2Sub(c, b))); - float vBC = c2Dot(b, c2Norm(c2Sub(b, c))); - float uCA = c2Dot(a, c2Norm(c2Sub(a, c))); - float vCA = c2Dot(c, c2Norm(c2Sub(c, a))); - float area = c2Det2(c2Norm(c2Sub(b, a)), c2Norm(c2Sub(c, a))); + 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; @@ -1016,17 +1088,10 @@ float c2GJK(const void* A, C2_TYPE typeA, const c2x* ax_ptr, const void* B, C2_T return dist; } -static C2_INLINE float c2Step(float t, const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, c2v* a, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, c2v* b, int use_radius, c2GJKCache* cache) -{ - c2x ax = *ax_ptr; - c2x bx = *bx_ptr; - ax.p = c2Add(ax.p, c2Mulvs(vA, t)); - bx.p = c2Add(bx.p, c2Mulvs(vB, t)); - float d = c2GJK(A, typeA, &ax, B, typeB, &bx, a, b, use_radius, NULL, cache); - return d; -} - -float c2TOI(const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, int use_radius, c2v* out_normal, c2v* out_contact_point, int* iterations) +// 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; @@ -1035,40 +1100,94 @@ float c2TOI(const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, const void* else ax = *ax_ptr; if (!bx_ptr) bx = c2xIdentity(); else bx = *bx_ptr; - c2v a, b, n; - c2GJKCache cache; - cache.count = 0; - float d = c2Step(t, A, typeA, &ax, vA, &a, B, typeB, &bx, vB, &b, use_radius, &cache); - c2v v = c2Sub(vB, vA); - n = c2SafeNorm(c2Sub(b, a)); - int iter = 0; - float eps = 1.0e-5f; - while (d > eps && t < 1) + 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 = false; + 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) { - ++iter; - float velocity_bound = c2Abs(c2Dot(c2Norm(c2Sub(b, a)), v)); - if (!velocity_bound) return 1; - float delta = d / velocity_bound; - t += delta * 0.95f; - c2v a0, b0; - d = c2Step(t, A, typeA, &ax, vA, &a0, B, typeB, &bx, vB, &b0, use_radius, &cache); - if (d * d >= eps) + 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) { - a = a0; - b = b0; - n = c2Sub(b, a); + case 2: c22(&s); break; + case 3: c23(&s); break; + } + + if (s.count == 3) { + return result; } + + v = c2L(&s); + result.iterations++; } - n = c2SafeNorm(n); - t = t >= 1 ? 1 : t; - c2v p = c2Mulvs(c2Add(a, b), 0.5f); + if (result.iterations == 0) { + result.hit = false; + } else { + 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 = true; + } - if (out_normal) *out_normal = n; - if (out_contact_point) *out_contact_point = p; - if (iterations) *iterations = iter; - return t; + return result; } int c2Hull(c2v* verts, int count) @@ -1143,6 +1262,97 @@ void c2MakePoly(c2Poly* p) 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); @@ -1170,6 +1380,22 @@ int c2AABBtoAABB(c2AABB A, c2AABB B) 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) { @@ -1358,42 +1584,65 @@ int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out) // rotate capsule to origin, along Y axis // rotate the ray same way - c2v yBb = c2MulmvT(M, c2Sub(B.b, B.a)); + 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)); - if (yAe.x * yAp.x < 0 || c2Min(c2Abs(yAe.x), c2Abs(yAp.x)) < B.r) - { - 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; + c2AABB capsule_bb; + capsule_bb.min = c2V(-B.r, 0); + capsule_bb.max = c2V(B.r, yBb.y); - // hit bottom half-circle - if (y < 0) - { - c2Circle C; - C.p = B.a; - C.r = B.r; - return c2RaytoCircle(A, C, out); + 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; } + } - // hit top-half circle - else if (y > yBb.y) - { - c2Circle C; - C.p = B.b; - C.r = B.r; - return c2RaytoCircle(A, C, out); + 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 middle of capsule + // hit the capsule prism else { - out->n = c > 0 ? M.x : c2Skew(M.y); - out->t = t * A.t; - return 1; + 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; + } } } @@ -1709,13 +1958,8 @@ static int c2Clip(c2v* seg, c2h h) #pragma warning(disable:4204) // nonstandard extension used: non-constant aggregate initializer #endif -// 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 c2SidePlanes(c2v* seg, c2x x, const c2Poly* p, int e, c2h* h) +static int c2SidePlanes(c2v* seg, c2v ra, c2v rb, c2h* h) { - c2v ra = c2Mulxv(x, p->verts[e]); - c2v rb = c2Mulxv(x, p->verts[e + 1 == p->count ? 0 : e + 1]); c2v in = c2Norm(c2Sub(rb, ra)); c2h left = { c2Neg(in), c2Dot(c2Neg(in), ra) }; c2h right = { in, c2Dot(in, rb) }; @@ -1728,6 +1972,16 @@ static int c2SidePlanes(c2v* seg, c2x x, const c2Poly* p, int e, c2h* h) 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; @@ -1776,6 +2030,23 @@ static void c2AntinormalFace(c2Capsule cap, const c2Poly* p, c2x x, int* face_ou *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; @@ -1783,63 +2054,103 @@ void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx_ptr, c2 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 == 0) + if (d < 1.0e-6f) { c2x bx = bx_ptr ? *bx_ptr : c2xIdentity(); - c2v n; - int index; - c2AntinormalFace(A, B, bx, &index, &n); - c2v seg[2] = { A.a, A.b }; - c2h h; - if (!c2SidePlanes(seg, bx, B, index, &h)) return; - c2KeepDeep(seg, h, m); - for (int i = 0; i < m->count; ++i) - { - m->depths[i] += c2Sign(m->depths) * A.r; - m->contact_points[i] = c2Add(m->contact_points[i], c2Mulvs(n, A.r)); - } - m->n = c2Neg(m->n); - } - - // shallow, use GJK results a and b to define manifold - else if (d < A.r) - { - c2x bx = bx_ptr ? *bx_ptr : c2xIdentity(); - c2v ab = c2Sub(b, a); - int face_case = 0; - + 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) { - c2v n = c2Mulrv(bx.r, B->norms[i]); - if (c2Parallel(c2Neg(ab), n, 5.0e-3f)) + 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) { - face_case = 1; - break; + sep = d; + index = i; } } - // 1 contact - if (!face_case) - { - one_contact: - m->count = 1; - m->n = c2Norm(ab); - m->contact_points[0] = c2Add(a, c2Mulvs(m->n, A.r)); - m->depths[0] = A.r - d; + // track axis of minimum separation + if (s0 > sep) { + sep = s0; + index = v0; + code = 1; } - // 2 contacts if laying on a polygon face nicely - else + if (s1 > sep) { + sep = s1; + index = v1; + code = 2; + } + + switch (code) { - c2v n; - int index; - c2AntinormalFace(A, B, bx, &index, &n); - c2v seg[2] = { c2Add(A.a, c2Mulvs(n, A.r)), c2Add(A.b, c2Mulvs(n, A.r)) }; + case 0: // poly face + { + c2v seg[2] = { A.a, A.b }; c2h h; - if (!c2SidePlanes(seg, bx, B, index, &h)) goto one_contact; + 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; } } @@ -1871,24 +2182,6 @@ static float c2CheckFaces(const c2Poly* A, c2x ax, const c2Poly* B, c2x bx, int* return sep; } -static C2_INLINE void c2Incident(c2v* incident, const c2Poly* ip, c2x ix, const c2Poly* rp, c2x rx, int re) -{ - c2v n = c2MulrvT(ix.r, c2Mulrv(rx.r, rp->norms[re])); - int index = ~0; - float min_dot = FLT_MAX; - for (int i = 0; i < ip->count; ++i) - { - float dot = c2Dot(n, 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]); -} - // 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 @@ -1928,9 +2221,9 @@ void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax_ptr, const c2Poly* B, c } c2v incident[2]; - c2Incident(incident, ip, ix, rp, rx, re); + c2Incident(incident, ip, ix, c2MulrvT(ix.r, c2Mulrv(rx.r, rp->norms[re]))); c2h rh; - if (!c2SidePlanes(incident, rx, rp, re, &rh)) return; + if (!c2SidePlanesFromPoly(incident, rx, rp, re, &rh)) return; c2KeepDeep(incident, rh, m); if (flip) m->n = c2Neg(m->n); } -- 2.39.2