Skip to content

Instantly share code, notes, and snippets.

@prime31

prime31/cute_c2_collisions.cs

Last active July 4, 2019 18:19
Show Gist options
  • Save prime31/d08abe4c6191af85e39c1a0fe88670bf to your computer and use it in GitHub Desktop.
Save prime31/d08abe4c6191af85e39c1a0fe88670bf to your computer and use it in GitHub Desktop.
Download ZIP
Raw
cute_c2_collisions.cs
const int C2_GJK_ITERS = 20;
public const int C2_MAX_POLYGON_VERTS = 8;
// position of impact p = ray.p + ray.d * raycast.t
public static c2v c2Impact(c2Ray ray, float t) => c2Add(ray.p, c2Mulvs(ray.d, t));
#region Wrapper Methods
public static unsafe bool c2Collided(void* A, c2x* ax, C2_TYPE typeA, void* B, c2x* bx, C2_TYPE typeB)
{
switch (typeA)
{
case C2_TYPE.C2_CIRCLE:
switch (typeB)
{
case C2_TYPE.C2_CIRCLE: return c2CircletoCircle(*(c2Circle*)A, *(c2Circle*)B);
case C2_TYPE.C2_AABB: return c2CircletoAABB(*(c2Circle*)A, *(c2AABB*)B);
case C2_TYPE.C2_CAPSULE: return c2CircletoCapsule(*(c2Circle*)A, *(c2Capsule*)B);
case C2_TYPE.C2_POLY: return c2CircletoPoly(*(c2Circle*)A, (c2Poly*)B, bx);
default: return false;
}
case C2_TYPE.C2_AABB:
switch (typeB)
{
case C2_TYPE.C2_CIRCLE: return c2CircletoAABB(*(c2Circle*)B, *(c2AABB*)A);
case C2_TYPE.C2_AABB: return c2AABBtoAABB(*(c2AABB*)A, *(c2AABB*)B);
case C2_TYPE.C2_CAPSULE: return c2AABBtoCapsule(*(c2AABB*)A, *(c2Capsule*)B);
case C2_TYPE.C2_POLY: return c2AABBtoPoly(*(c2AABB*)A, (c2Poly*)B, bx);
default: return false;
}
case C2_TYPE.C2_CAPSULE:
switch (typeB)
{
case C2_TYPE.C2_CIRCLE: return c2CircletoCapsule(*(c2Circle*)B, *(c2Capsule*)A);
case C2_TYPE.C2_AABB: return c2AABBtoCapsule(*(c2AABB*)B, *(c2Capsule*)A);
case C2_TYPE.C2_CAPSULE: return c2CapsuletoCapsule(*(c2Capsule*)A, *(c2Capsule*)B);
case C2_TYPE.C2_POLY: return c2CapsuletoPoly(*(c2Capsule*)A, (c2Poly*)B, bx);
default: return false;
}
case C2_TYPE.C2_POLY:
switch (typeB)
{
case C2_TYPE.C2_CIRCLE: return c2CircletoPoly(*(c2Circle*)B, (c2Poly*)A, ax);
case C2_TYPE.C2_AABB: return c2AABBtoPoly(*(c2AABB*)B, (c2Poly*)A, ax);
case C2_TYPE.C2_CAPSULE: return c2CapsuletoPoly(*(c2Capsule*)B, (c2Poly*)A, ax);
case C2_TYPE.C2_POLY: return c2PolytoPoly((c2Poly*)A, ax, (c2Poly*)B, bx);
default: return false;
}
default:
return false;
}
}
public static unsafe void c2Collide(void* A, c2x* ax, C2_TYPE typeA, void* B, c2x* bx, C2_TYPE typeB, c2Manifold* m)
{
m->count = 0;
switch (typeA)
{
case C2_TYPE.C2_CIRCLE:
switch (typeB)
{
case C2_TYPE.C2_CIRCLE: c2CircletoCircleManifold(*(c2Circle*)A, *(c2Circle*)B, m); break;
case C2_TYPE.C2_AABB: c2CircletoAABBManifold(*(c2Circle*)A, *(c2AABB*)B, m); break;
case C2_TYPE.C2_CAPSULE: c2CircletoCapsuleManifold(*(c2Circle*)A, *(c2Capsule*)B, m); break;
case C2_TYPE.C2_POLY: c2CircletoPolyManifold(*(c2Circle*)A, (c2Poly*)B, bx, m); break;
}
break;
case C2_TYPE.C2_AABB:
switch (typeB)
{
case C2_TYPE.C2_CIRCLE: c2CircletoAABBManifold(*(c2Circle*)B, *(c2AABB*)A, m); m->n = c2Neg(m->n); break;
case C2_TYPE.C2_AABB: c2AABBtoAABBManifold(*(c2AABB*)A, *(c2AABB*)B, m); break;
case C2_TYPE.C2_CAPSULE: c2AABBtoCapsuleManifold(*(c2AABB*)A, *(c2Capsule*)B, m); break;
case C2_TYPE.C2_POLY: c2AABBtoPolyManifold(*(c2AABB*)A, (c2Poly*)B, bx, m); break;
}
break;
case C2_TYPE.C2_CAPSULE:
switch (typeB)
{
case C2_TYPE.C2_CIRCLE: c2CircletoCapsuleManifold(*(c2Circle*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
case C2_TYPE.C2_AABB: c2AABBtoCapsuleManifold(*(c2AABB*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
case C2_TYPE.C2_CAPSULE: c2CapsuletoCapsuleManifold(*(c2Capsule*)A, *(c2Capsule*)B, m); break;
case C2_TYPE.C2_POLY: c2CapsuletoPolyManifold(*(c2Capsule*)A, (c2Poly*)B, bx, m); break;
}
break;
case C2_TYPE.C2_POLY:
switch (typeB)
{
case C2_TYPE.C2_CIRCLE: c2CircletoPolyManifold(*(c2Circle*)B, (c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
case C2_TYPE.C2_AABB: c2AABBtoPolyManifold(*(c2AABB*)B, (c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
case C2_TYPE.C2_CAPSULE: c2CapsuletoPolyManifold(*(c2Capsule*)B, (c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
case C2_TYPE.C2_POLY: c2PolytoPolyManifold((c2Poly*)A, ax, (c2Poly*)B, bx, m); break;
}
break;
}
}
public static unsafe int c2CastRay(c2Ray A, void* B, c2x* bx, C2_TYPE typeB, c2Raycast* outt)
{
switch (typeB)
{
case C2_TYPE.C2_CIRCLE: return c2RaytoCircle(A, *(c2Circle*)B, outt);
case C2_TYPE.C2_AABB: return c2RaytoAABB(A, *(c2AABB*)B, outt);
case C2_TYPE.C2_CAPSULE: return c2RaytoCapsule(A, *(c2Capsule*)B, outt);
case C2_TYPE.C2_POLY: return c2RaytoPoly(A, (c2Poly*)B, bx, outt);
}
return 0;
}
#endregion
public struct c2Proxy
{
public float radius;
public int count;
public unsafe fixed byte _verts[sizeof(float) * 2 * tiny.C2_MAX_POLYGON_VERTS]; // float2[8]
public unsafe c2v* GetVerts()
{
fixed (c2Proxy* proxy = &this)
return (c2v*)(&proxy->_verts[0]);
}
}
public struct c2sv
{
public c2v sA;
public c2v sB;
public c2v p;
public float u;
public int iA;
public int iB;
}
public struct c2Simplex
{
public c2sv a, b, c, d;
public float div;
public int count;
}
public static unsafe void c2MakeProxy(void* shape, C2_TYPE type, out c2Proxy p)
{
p = new c2Proxy();
var verts = p.GetVerts();
switch (type)
{
case C2_TYPE.C2_CIRCLE:
{
var c = Unsafe.Read<c2Circle>(shape);
p.radius = c.r;
p.count = 1;
verts[0] = c.p;
}
break;
case C2_TYPE.C2_AABB:
{
var bb = Unsafe.Read<c2AABB>(shape);
p.radius = 0;
p.count = 4;
c2BBVerts(verts, ref bb);
}
break;
case C2_TYPE.C2_CAPSULE:
{
var c = Unsafe.Read<c2Capsule>(shape);
p.radius = c.r;
p.count = 2;
verts[0] = c.a;
verts[1] = c.b;
}
break;
case C2_TYPE.C2_POLY:
{
var poly = Unsafe.Read<c2Poly>(shape);
p.radius = 0;
p.count = poly.count;
c2v* vertices = (c2v*)(&poly._verts[0]);
for (var i = 0; i < p.count; ++i)
verts[i] = vertices[i];
}
break;
}
}
public static unsafe int c2Support(c2v* verts, int count, c2v d)
{
var imax = 0;
var dmax = c2Dot(verts[0], d);
for (int i = 1; i < count; ++i)
{
var dot = c2Dot(verts[i], d);
if (dot > dmax)
{
imax = i;
dmax = dot;
}
}
return imax;
}
public static c2v c2L(ref c2Simplex s)
{
var den = 1.0f / s.div;
switch (s.count)
{
case 1: return s.a.p;
case 2: return c2Add(c2Mulvs(s.a.p, (den * s.a.u)), c2Mulvs(s.b.p, (den * s.b.u)));
case 3: return c2Add(c2Add(c2Mulvs(s.a.p, (den * s.a.u)), c2Mulvs(s.b.p, (den * s.b.u))), c2Mulvs(s.c.p, (den * s.c.u)));
default: return c2V(0, 0);
}
}
public static void c2Witness(ref c2Simplex s, out c2v a, out 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 = c2Add(c2Mulvs(s.a.sA, (den * s.a.u)), c2Mulvs(s.b.sA, (den * s.b.u)));
b = c2Add(c2Mulvs(s.a.sB, (den * s.a.u)), c2Mulvs(s.b.sB, (den * s.b.u)));
break;
case 3:
a = c2Add(c2Add(c2Mulvs(s.a.sA, (den * s.a.u)), c2Mulvs(s.b.sA, (den * s.b.u))), c2Mulvs(s.c.sA, (den * s.c.u)));
b = c2Add(c2Add(c2Mulvs(s.a.sB, (den * s.a.u)), c2Mulvs(s.b.sB, (den * s.b.u))), c2Mulvs(s.c.sB, (den * s.c.u)));
break;
default:
a = c2V(0, 0);
b = c2V(0, 0);
break;
}
}
public static c2v c2D(ref c2Simplex s)
{
switch (s.count)
{
case 1: return c2Neg(s.a.p);
case 2:
{
var 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);
}
}
public static void c22(ref c2Simplex s)
{
c2v a = s.a.p;
c2v b = s.b.p;
var u = c2Dot(b, c2Norm(c2Sub(b, a)));
var v = c2Dot(a, c2Norm(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;
}
}
public static void c23(ref c2Simplex s)
{
c2v a = s.a.p;
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 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;
}
}
public static float c2GJKSimplexMetric(ref 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));
}
}
public static unsafe float c2GJK(void* A, C2_TYPE typeA, c2x* ax_ptr, void* B, C2_TYPE typeB, c2x* bx_ptr, c2v* outA, c2v* outB, int use_radius, int* iterations, c2GJKCache* cache)
{
c2x ax;
c2x bx;
if (ax_ptr == null)
ax = c2xIdentity();
else
ax = *ax_ptr;
if (bx_ptr == null)
bx = c2xIdentity();
else bx = *bx_ptr;
c2MakeProxy(A, typeA, out var pA);
c2MakeProxy(B, typeB, out var pB);
var pAverts = pA.GetVerts();
var pBverts = pB.GetVerts();
c2Simplex s = new c2Simplex();
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 != null)
{
var cache_was_good = cache->count > 0;
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, pAverts[iA]);
c2v sB = c2Mulxv(bx, pBverts[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(ref 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 == 0)
{
s.a.iA = 0;
s.a.iB = 0;
s.a.sA = c2Mulxv(ax, pAverts[0]);
s.a.sB = c2Mulxv(bx, pBverts[0]);
s.a.p = c2Sub(s.a.sB, s.a.sA);
s.a.u = 1.0f;
s.div = 1.0f;
s.count = 1;
}
var saveA = new int[3];
var saveB = new int[3];
var save_count = 0;
float d0 = float.MaxValue;
float d1 = float.MaxValue;
int iter = 0;
var hit = false;
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(ref s); break;
case 3: c23(ref s); break;
}
if (s.count == 3)
{
hit = true;
break;
}
c2v p = c2L(ref s);
d1 = c2Dot(p, p);
if (d1 > d0) break;
d0 = d1;
c2v d = c2D(ref s);
if (c2Dot(d, d) < float.Epsilon * float.Epsilon)
break;
int iA = c2Support(pAverts, pA.count, c2MulrvT(ax.r, c2Neg(d)));
c2v sA = c2Mulxv(ax, pAverts[iA]);
int iB = c2Support(pBverts, pB.count, c2MulrvT(bx.r, d));
c2v sB = c2Mulxv(bx, pBverts[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);
var dup = false;
for (int i = 0; i < save_count; ++i)
{
if (iA == saveA[i] && iB == saveB[i])
{
dup = true;
break;
}
}
if (dup)
break;
++s.count;
++iter;
}
c2Witness(ref s, out var a, out var b);
float dist = c2Len(c2Sub(a, b));
if (hit)
{
a = b;
dist = 0;
}
else if (use_radius == 1)
{
float rA = pA.radius;
float rB = pB.radius;
if (dist > rA + rB && dist > float.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 != null)
{
cache->metric = c2GJKSimplexMetric(ref 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 != null)
*outA = a;
if (outB != null)
*outB = b;
if (iterations != null)
*iterations = iter;
return dist;
}
public static unsafe float c2Step(float t, void* A, C2_TYPE typeA, c2x* ax_ptr, c2v vA, c2v* a, void* B, C2_TYPE typeB, 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;
}
public static unsafe float c2TOI(void* A, C2_TYPE typeA, c2x* ax_ptr, c2v vA, void* B, C2_TYPE typeB, c2x* bx_ptr, c2v vB, int use_radius, c2v* out_normal, c2v* out_contact_point, int* iterations)
{
float t = 0;
c2x ax;
c2x bx;
if (ax_ptr == null)
ax = c2xIdentity();
else
ax = *ax_ptr;
if (bx_ptr == null)
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-4f;
while (d > eps && t < 1)
{
++iter;
float velocity_bound = c2Abs(c2Dot(c2Norm(c2Sub(b, a)), v));
if (velocity_bound == 0)
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)
{
a = a0;
b = b0;
n = c2Sub(b, a);
}
else break;
}
n = c2SafeNorm(n);
t = t >= 1 ? 1 : t;
c2v p = c2Mulvs(c2Add(a, b), 0.5f);
if (out_normal != null)
*out_normal = n;
if (out_contact_point != null)
*out_contact_point = p;
if (iterations != null)
*iterations = iter;
return t >= 1 ? 1 : t;
}
public static unsafe 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;
}
var hull = stackalloc int[C2_MAX_POLYGON_VERTS];
int out_count = 0;
int index = right;
while (true)
{
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;
}
var hull_verts = new c2v[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);
// for (int i = 0; i < out_count; ++i)
// verts[i] = hull_verts[i];
// memcpy the hard way
var handle = GCHandle.Alloc(hull_verts, GCHandleType.Pinned);
var addr = handle.AddrOfPinnedObject();
var size = Unsafe.SizeOf<c2v>();
var destAddr = (byte*)verts;
var srcAddr = (byte*)addr;
Unsafe.CopyBlock(destAddr, srcAddr, (uint)(out_count * size));
handle.Free();
return out_count;
}
static unsafe 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));
}
}
public static unsafe void c2MakePoly(c2Poly* p)
{
c2v* vertices = (c2v*)(&p->_verts[0]);
c2v* normals = (c2v*)(&p->_norms[0]);
p->count = c2Hull(vertices, p->count);
c2Norms(vertices, normals, p->count);
}
#region Collision Hit Tests
public static bool 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;
}
public static bool 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;
}
public static bool c2AABBtoAABB(c2AABB A, c2AABB B)
{
var d0 = B.max.x < A.min.x;
var d1 = A.max.x < B.min.x;
var d2 = B.max.y < A.min.y;
var d3 = A.max.y < B.min.y;
return !(d0 | d1 | d2 | d3);
}
// see: http://www.randygaul.net/2014/07/23/distance-point-to-line-segment/
public static bool 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;
}
public static unsafe bool c2AABBtoCapsule(c2AABB A, c2Capsule B)
{
if (c2GJK(&A, C2_TYPE.C2_AABB, null, &B, C2_TYPE.C2_CAPSULE, null, null, null, 1, null, null) != 0)
return true;
return false;
}
public static unsafe bool c2CapsuletoCapsule(c2Capsule A, c2Capsule B)
{
if (c2GJK(&A, C2_TYPE.C2_CAPSULE, null, &B, C2_TYPE.C2_CAPSULE, null, null, null, 1, null, null) != 0)
return false;
return true;
}
public static unsafe bool c2CircletoPoly(c2Circle A, c2Poly* B, c2x* bx)
{
if (c2GJK(&A, C2_TYPE.C2_CIRCLE, null, B, C2_TYPE.C2_POLY, bx, null, null, 1, null, null) != 0)
return false;
return true;
}
public static unsafe bool c2AABBtoPoly(c2AABB A, c2Poly* B, c2x* bx)
{
if (c2GJK(&A, C2_TYPE.C2_AABB, null, B, C2_TYPE.C2_POLY, bx, null, null, 1, null, null) != 0)
return false;
return true;
}
public static unsafe bool c2CapsuletoPoly(c2Capsule A, c2Poly* B, c2x* bx)
{
if (c2GJK(&A, C2_TYPE.C2_CAPSULE, null, B, C2_TYPE.C2_POLY, bx, null, null, 1, null, null) != 0)
return false;
return true;
}
public static unsafe bool c2PolytoPoly(c2Poly* A, c2x* ax, c2Poly* B, c2x* bx)
{
if (c2GJK(A, C2_TYPE.C2_POLY, ax, B, C2_TYPE.C2_POLY, bx, null, null, 1, null, null) != 0)
return false;
return true;
}
#endregion
#region Raycasts
public static unsafe int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* outt)
{
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)
{
outt->t = t;
c2v impact = c2Impact(A, t);
outt->n = c2Norm(c2Sub(impact, p));
return 1;
}
return 0;
}
public static unsafe int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* outt)
{
c2v inv = c2V(1.0f / A.d.x, 1.0f / A.d.y);
c2v d0 = c2Mulvv(c2Sub(B.min, A.p), inv);
c2v d1 = c2Mulvv(c2Sub(B.max, A.p), inv);
c2v v0 = c2Minv(d0, d1);
c2v v1 = c2Maxv(d0, d1);
float lo = c2Hmax(v0);
float hi = c2Hmin(v1);
if (hi >= 0 && hi >= lo && lo <= A.t)
{
c2v c = c2Mulvs(c2Add(B.min, B.max), 0.5f);
c = c2Sub(c2Impact(A, lo), c);
c2v abs_c = c2Absv(c);
if (abs_c.x > abs_c.y)
outt->n = c2V(c2Sign(c.x), 0);
else
outt->n = c2V(0, c2Sign(c.y));
outt->t = lo;
return 1;
}
return 0;
}
public static unsafe int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* outt)
{
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 yBb = c2MulmvT(M, c2Sub(B.b, B.a));
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;
// hit bottom half-circle
if (y < 0)
{
c2Circle C;
C.p = B.a;
C.r = B.r;
return c2RaytoCircle(A, C, outt);
}
// hit top-half circle
else if (y > yBb.y)
{
c2Circle C;
C.p = B.b;
C.r = B.r;
return c2RaytoCircle(A, C, outt);
}
// hit the middle of capsule
else
{
outt->n = c > 0 ? M.x : c2Skew(M.y);
outt->t = t * A.t;
return 1;
}
}
return 0;
}
public static unsafe int c2RaytoPoly(c2Ray A, c2Poly* B, c2x* bx_ptr, c2Raycast* outt)
{
c2x bx = bx_ptr != null ? *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;
c2v* vertices = (c2v*)(&B->_verts[0]);
c2v* normals = (c2v*)(&B->_norms[0]);
// test ray to each plane, tracking lo/hi times of intersection
for (int i = 0; i < B->count; ++i)
{
float num = c2Dot(normals[i], c2Sub(vertices[i], p));
float den = c2Dot(normals[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 != -1)
{
outt->t = lo;
outt->n = c2Mulrv(bx.r, normals[index]);
return 1;
}
return 0;
}
#endregion
#region Collision Manifolds
public static unsafe void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m)
{
c2v* contact_points = (c2v*)(&m->_contact_points[0]);
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;
contact_points[0] = c2Sub(B.p, c2Mulvs(n, B.r));
m->n = n;
}
}
public static unsafe void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m)
{
c2v* contact_points = (c2v*)(&m->_contact_points[0]);
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;
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;
contact_points[0] = c2Sub(A.p, c2Mulvs(n, depth));
m->n = n;
}
}
}
public static unsafe void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m)
{
c2v* contact_points = (c2v*)(&m->_contact_points[0]);
m->count = 0;
c2v a, b;
float r = A.r + B.r;
float d = c2GJK(&A, C2_TYPE.C2_CIRCLE, null, &B, C2_TYPE.C2_CAPSULE, null, &a, &b, 0, null, null);
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;
contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
m->n = n;
}
}
public static unsafe void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m)
{
c2v* contact_points = (c2v*)(&m->_contact_points[0]);
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;
contact_points[0] = p;
m->depths[0] = depth;
m->n = n;
}
public static unsafe void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m)
{
c2Poly p;
c2v* vertices = (c2v*)(&p._verts[0]);
c2v* normals = (c2v*)(&p._norms[0]);
c2BBVerts(vertices, ref A);
p.count = 4;
c2Norms(vertices, normals, 4);
m->count = 0;
c2CapsuletoPolyManifold(B, &p, null, m);
m->n = c2Neg(m->n);
}
public static unsafe void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m)
{
c2v* contact_points = (c2v*)(&m->_contact_points[0]);
m->count = 0;
c2v a, b;
float r = A.r + B.r;
float d = c2GJK(&A, C2_TYPE.C2_CAPSULE, null, &B, C2_TYPE.C2_CAPSULE, null, &a, &b, 0, null, null);
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;
contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
m->n = n;
}
}
static unsafe c2h c2PlaneAt(c2Poly* p, int i)
{
c2v* vertices = (c2v*)(&p->_verts[0]);
c2v* normals = (c2v*)(&p->_norms[0]);
c2h h;
h.n = normals[i];
h.d = c2Dot(normals[i], vertices[i]);
return h;
}
public static unsafe void c2CircletoPolyManifold(c2Circle A, c2Poly* B, c2x* bx_tr, c2Manifold* m)
{
c2v* contact_points = (c2v*)(&m->_contact_points[0]);
m->count = 0;
c2v a, b;
float d = c2GJK(&A, C2_TYPE.C2_CIRCLE, null, B, C2_TYPE.C2_POLY, bx_tr, &a, &b, 0, null, null);
// 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;
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 != null ? *bx_tr : c2xIdentity();
float sep = float.MinValue;
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 h2 = c2PlaneAt(B, index);
c2v p = c2Project(h2, local);
m->count = 1;
contact_points[0] = c2Mulxv(bx, p);
m->depths[0] = A.r - sep;
c2v* normals = (c2v*)(&B->_norms[0]);
m->n = c2Neg(c2Mulrv(bx.r, normals[index]));
}
}
// Forms a c2Poly and uses c2PolytoPolyManifold
public static unsafe void c2AABBtoPolyManifold(c2AABB A, c2Poly* B, c2x* bx, c2Manifold* m)
{
c2v* vertices = (c2v*)(&B->_verts[0]);
c2v* normals = (c2v*)(&B->_norms[0]);
m->count = 0;
c2Poly p;
c2BBVerts(vertices, ref A);
p.count = 4;
c2Norms(vertices, normals, 4);
c2PolytoPolyManifold(&p, null, B, bx, m);
}
// clip a segment to a plane
static unsafe int c2Clip(c2v* seg, c2h h)
{
var outt = new c2v[2];
int sp = 0;
float d0, d1;
if ((d0 = c2Dist(h, seg[0])) < 0)
outt[sp++] = seg[0];
if ((d1 = c2Dist(h, seg[1])) < 0)
outt[sp++] = seg[1];
if (d0 == 0 && d1 == 0)
{
outt[sp++] = seg[0];
outt[sp++] = seg[1];
}
else if (d0 * d1 <= 0)
outt[sp++] = c2Intersect(seg[0], seg[1], d0, d1);
seg[0] = outt[0]; seg[1] = outt[1];
return sp;
}
// 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 unsafe int c2SidePlanes(c2v* seg, c2x x, c2Poly* p, int e, c2h* h)
{
c2v* vertices = (c2v*)(&p->_verts[0]);
c2v ra = c2Mulxv(x, vertices[e]);
c2v rb = c2Mulxv(x, vertices[e + 1 == p->count ? 0 : e + 1]);
c2v inn = c2Norm(c2Sub(rb, ra));
c2h left = new c2h(c2Neg(inn), c2Dot(c2Neg(inn), ra));
c2h right = new c2h(inn, c2Dot(inn, rb));
if (c2Clip(seg, left) < 2)
return 0;
if (c2Clip(seg, right) < 2)
return 0;
if (h != null)
{
h->n = c2CCW90(inn);
h->d = c2Dot(c2CCW90(inn), ra);
}
return 1;
}
static unsafe void c2KeepDeep(c2v* seg, c2h h, c2Manifold* m)
{
c2v* contact_points = (c2v*)(&m->_contact_points[0]);
int cp = 0;
for (int i = 0; i < 2; ++i)
{
c2v p = seg[i];
float d = c2Dist(h, p);
if (d <= 0)
{
contact_points[cp] = p;
m->depths[cp] = -d;
++cp;
}
}
m->count = cp;
m->n = h.n;
}
static 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));
return c2Add(A.b, c2Mulvs(dir, A.r));
}
static unsafe void c2AntinormalFace(c2Capsule cap, c2Poly* p, c2x x, int* face_out, c2v* n_out)
{
float sep = float.MinValue;
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;
}
public static unsafe void c2CapsuletoPolyManifold(c2Capsule A, c2Poly* B, c2x* bx_ptr, c2Manifold* m)
{
c2v* contact_points = (c2v*)(&m->_contact_points[0]);
m->count = 0;
c2v a, b;
float d = c2GJK(&A, C2_TYPE.C2_CAPSULE, null, B, C2_TYPE.C2_POLY, bx_ptr, &a, &b, 0, null, null);
// deep, treat as segment to poly collision
if (d == 0)
{
c2x bx = bx_ptr != null ? *bx_ptr : c2xIdentity();
c2v n;
int index;
c2AntinormalFace(A, B, bx, &index, &n);
var seg = new c2v[] { A.a, A.b };
c2h h;
fixed (c2v* seg_ptr = &seg[0])
{
if (c2SidePlanes(seg_ptr, bx, B, index, &h) != 0)
return;
c2KeepDeep(seg_ptr, h, m);
}
for (int i = 0; i < m->count; ++i)
contact_points[i] = c2Add(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 != null ? *bx_ptr : c2xIdentity();
c2v ab = c2Sub(b, a);
int face_case = 0;
c2v* normals = (c2v*)(&B->_norms[0]);
for (int i = 0; i < B->count; ++i)
{
c2v n = c2Mulrv(bx.r, normals[i]);
if (c2Parallel(c2Neg(ab), n, 5.0e-3f) > 0)
{
face_case = 1;
break;
}
}
one_contact:
// 1 contact
if (face_case != 0)
{
m->count = 1;
m->n = c2Norm(ab);
contact_points[0] = c2Add(a, c2Mulvs(m->n, A.r));
m->depths[0] = A.r - d;
}
// 2 contacts if laying on a polygon face nicely
else
{
c2v n;
int index;
c2AntinormalFace(A, B, bx, &index, &n);
var seg = new c2v[] { c2Add(A.a, c2Mulvs(n, A.r)), c2Add(A.b, c2Mulvs(n, A.r)) };
c2h h;
fixed (c2v* seg_ptr = &seg[0])
{
if (c2SidePlanes(seg_ptr, bx, B, index, &h) != 0)
{
face_case = 1; // need this to get through the if statement
goto one_contact;
}
c2KeepDeep(seg_ptr, h, m);
m->n = c2Neg(m->n);
}
}
}
}
static unsafe float c2CheckFaces(c2Poly* A, c2x ax, c2Poly* B, c2x bx, int* face_index)
{
c2x b_in_a = c2MulxxT(ax, bx);
c2x a_in_b = c2MulxxT(bx, ax);
float sep = float.MinValue;
int index = ~0;
c2v* verts = (c2v*)(&B->_verts[0]);
for (int i = 0; i < A->count; ++i)
{
c2h h = c2PlaneAt(A, i);
int idx = c2Support(verts, B->count, c2Mulrv(a_in_b.r, c2Neg(h.n)));
c2v p = c2Mulxv(b_in_a, verts[idx]);
float d = c2Dist(h, p);
if (d > sep)
{
sep = d;
index = i;
}
}
*face_index = index;
return sep;
}
static unsafe void c2Incident(c2v* incident, c2Poly* ip, c2x ix, c2Poly* rp, c2x rx, int re)
{
c2v* rp_norms = (c2v*)(&rp->_norms[0]);
c2v* ip_norms = (c2v*)(&ip->_norms[0]);
c2v* ip_verts = (c2v*)(&ip->_verts[0]);
c2v n = c2MulrvT(ix.r, c2Mulrv(rx.r, rp_norms[re]));
int index = ~0;
float min_dot = float.MaxValue;
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]);
}
public static unsafe void c2PolytoPolyManifold(c2Poly* A, c2x* ax_ptr, c2Poly* B, c2x* bx_ptr, c2Manifold* m)
{
m->count = 0;
c2x ax = ax_ptr != null ? *ax_ptr : c2xIdentity();
c2x bx = bx_ptr != null ? *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;
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;
}
// var incident = new c2v[2];
var incident = stackalloc c2v[2];
c2Incident(incident, ip, ix, rp, rx, re);
c2h rh;
if (c2SidePlanes(incident, rx, rp, re, &rh) > 0)
return;
c2KeepDeep(incident, rh, m);
if (flip > 0)
m->n = c2Neg(m->n);
}
#endregion
Raw
cute_c2_maths.cs
#region Primitive ops
static float c2Sin(float radians) => Mathf.sin(radians);
static float c2Cos(float radians) => Mathf.cos(radians);
static float c2Sqrt(float a) => math.sqrt(a);
static float c2Min(float a, float b) => ((a) < (b) ? (a) : (b));
static int c2Min(int a, int b) => ((a) < (b) ? (a) : (b));
static float c2Max(float a, float b) => ((a) > (b) ? (a) : (b));
static float c2Abs(float a) => ((a) < 0 ? -(a) : (a));
static float c2Clamp(float a, float lo, float hi) => c2Max(lo, c2Min(a, hi));
static unsafe void c2SinCos(float radians, float* s, float* c)
{
*c = c2Cos(radians);
*s = c2Sin(radians);
}
static float c2Sign(float a) => (a < 0 ? -1.0f : 1.0f);
#endregion
// 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".
#region vector ops
public static c2v c2V(float x, float y) { c2v a; a.x = x; a.y = y; return a; }
public static c2v c2Add(c2v a, c2v b) { a.x += b.x; a.y += b.y; return a; }
public static c2v c2Sub(c2v a, c2v b) { a.x -= b.x; a.y -= b.y; return a; }
public static float c2Dot(c2v a, c2v b) { return a.x * b.x + a.y * b.y; }
public static c2v c2Mulvs(c2v a, float b) { a.x *= b; a.y *= b; return a; }
public static c2v c2Mulvv(c2v a, c2v b) { a.x *= b.x; a.y *= b.y; return a; }
public static c2v c2Div(c2v a, float b) { return c2Mulvs(a, 1.0f / b); }
public static c2v c2Skew(c2v a) { c2v b; b.x = -a.y; b.y = a.x; return b; }
public static c2v c2CCW90(c2v a) { c2v b; b.x = a.y; b.y = -a.x; return b; }
public static float c2Det2(c2v a, c2v b) { return a.x * b.y - a.y * b.x; }
public static c2v c2Minv(c2v a, c2v b) { return c2V(c2Min(a.x, b.x), c2Min(a.y, b.y)); }
public static c2v c2Maxv(c2v a, c2v b) { return c2V(c2Max(a.x, b.x), c2Max(a.y, b.y)); }
public static c2v c2Clampv(c2v a, c2v lo, c2v hi) { return c2Maxv(lo, c2Minv(a, hi)); }
public static c2v c2Absv(c2v a) { return c2V(c2Abs(a.x), c2Abs(a.y)); }
public static float c2Hmin(c2v a) { return c2Min(a.x, a.y); }
public static float c2Hmax(c2v a) { return c2Max(a.x, a.y); }
public static float c2Len(c2v a) { return c2Sqrt(c2Dot(a, a)); }
public static c2v c2Norm(c2v a) { return c2Div(a, c2Len(a)); }
public static c2v c2SafeNorm(c2v a) { float sq = c2Dot(a, a); return sq != 0 ? c2Div(a, c2Len(a)) : c2V(0, 0); }
public static c2v c2Neg(c2v a) { return c2V(-a.x, -a.y); }
public static c2v c2Lerp(c2v a, c2v b, float t) { return c2Add(a, c2Mulvs(c2Sub(b, a), t)); }
public static 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;
}
#endregion
#region rotation ops
public static unsafe c2r c2Rot(float radians) { c2r r; c2SinCos(radians, &r.s, &r.c); return r; }
public static c2r c2RotIdentity() { c2r r; r.c = 1.0f; r.s = 0; return r; }
public static c2v c2RotX(c2r r) { return c2V(r.c, r.s); }
public static c2v c2RotY(c2r r) { return c2V(-r.s, r.c); }
public static c2v c2Mulrv(c2r a, c2v b) { return c2V(a.c * b.x - a.s * b.y, a.s * b.x + a.c * b.y); }
public static c2v c2MulrvT(c2r a, c2v b) { return c2V(a.c * b.x + a.s * b.y, -a.s * b.x + a.c * b.y); }
public static 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; }
public static 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; }
public static 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; }
public static 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; }
public static c2m c2Mulmm(c2m a, c2m b) { c2m c; c.x = c2Mulmv(a, b.x); c.y = c2Mulmv(a, b.y); return c; }
public static c2m c2MulmmT(c2m a, c2m b) { c2m c; c.x = c2MulmvT(a, b.x); c.y = c2MulmvT(a, b.y); return c; }
#endregion
#region transform ops
public static c2x c2xIdentity() { c2x x; x.p = c2V(0, 0); x.r = c2RotIdentity(); return x; }
public static c2v c2Mulxv(c2x a, c2v b) { return c2Add(c2Mulrv(a.r, b), a.p); }
public static c2v c2MulxvT(c2x a, c2v b) { return c2MulrvT(a.r, c2Sub(b, a.p)); }
public static 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; }
public static 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; }
public static c2x c2Transform(c2v p, float radians) { c2x x; x.r = c2Rot(radians); x.p = p; return x; }
#endregion
#region halfspace ops
public static c2v c2Origin(c2h h) { return c2Mulvs(h.n, h.d); }
public static float c2Dist(c2h h, c2v p) { return c2Dot(h.n, p) - h.d; }
public static c2v c2Project(c2h h, c2v p) { return c2Sub(p, c2Mulvs(h.n, c2Dist(h, p))); }
public static 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; }
public static 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; }
public static c2v c2Intersect(c2v a, c2v b, float da, float db) { return c2Add(a, c2Mulvs(c2Sub(b, a), (da / (da - db)))); }
#endregion
public static unsafe void c2BBVerts(c2v* verts, ref c2AABB bb)
{
verts[0] = bb.min;
verts[1] = c2V(bb.max.x, bb.min.y);
verts[2] = bb.max;
verts[3] = c2V(bb.min.x, bb.max.y);
}
Raw
cute_c2_structs.cs
// 2d vector
public struct c2v
{
public float x;
public float y;
public c2v(float x, float y)
{
this.x = x;
this.y = y;
}
}
// 2d rotation composed of cos/sin pair
public struct c2r
{
public float c;
public float s;
}
// 2d rotation matrix
public struct c2m
{
public c2v x;
public c2v y;
}
public struct c2x
{
public c2v p;
public c2r r;
}
// 2d halfspace (aka plane, aka line)
public struct c2h
{
public c2v n; // normal, normalized
public float d; // distance to origin from plane, or ax + by = d
public c2h(c2v n, float d)
{
this.n = n;
this.d = d;
}
}
public struct c2Circle
{
public c2v p;
public float r;
}
public struct c2AABB
{
public c2v min;
public c2v max;
}
// a capsule is defined as a line segment (from a to b) and radius r
public struct c2Capsule
{
public c2v a;
public c2v b;
public float r;
}
public struct c2Poly
{
public int count;
public unsafe fixed byte _verts[sizeof(float) * 2 * tiny.C2_MAX_POLYGON_VERTS]; // c2v[8]
public unsafe fixed byte _norms[sizeof(float) * 2 * tiny.C2_MAX_POLYGON_VERTS]; // c2v[8]
public unsafe c2v* GetVerts()
{
fixed (c2Poly* poly = &this)
return (c2v*)(&poly->_verts[0]);
}
}
public struct c2Ray
{
public c2v p; // position
public c2v d; // direction (normalized)
public float t; // distance along d from position p to find endpoint of ray
}
public struct c2Raycast
{
public float t; // time of impact
public c2v n; // normal of surface at impact (unit length)
}
public unsafe struct c2Manifold
{
public int count;
public fixed float depths[2];
public unsafe fixed byte _contact_points[sizeof(float) * 2 * 2]; // c2v[2]
// always points from shape A to shape B (first and second shapes passed into any of the c2***to***Manifold functions)
public c2v n;
public c2v* contactPoints
{
get
{
fixed (c2Manifold* manifold = &this)
return (c2v*)(&manifold->_contact_points[0]);
}
}
}
public enum C2_TYPE
{
C2_NONE,
C2_CIRCLE,
C2_AABB,
C2_CAPSULE,
C2_POLY
}
public unsafe struct c2GJKCache
{
public float metric;
public int count;
public fixed int iA[3];
public fixed int iB[3];
public float div;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment