Vector2 Intersect(Body a, Body b)
{
Vector2 summary = Vector2.Zero;
int cnt = 0;
foreach (Vector2 v in a.shape)
{
int c = b.shape.Count;
bool In = false;
for (int i = 0; i < c - 2; i++)
{
if (isin(b.shape[0], b.shape[(i + 1) % c], b.shape[(i + 2) % c], v + a.position - b.position))
In = true;
}
if (In)
{
summary += v + a.position;
cnt++;
}
}
foreach (Vector2 v in b.shape)
{
int c = a.shape.Count;
bool In = false;
for (int i = 0; i < c - 2; i++)
{
if (isin(a.shape[0], a.shape[(i + 1) % c], a.shape[(i + 2) % c], v + b.position - a.position))
In = true;
}
if (In)
{
summary += v + b.position;
cnt++;
}
}
if (cnt == 0) return new Vector2(100000, 100000);
summary /= cnt;
return summary;
}
void Solve(Body a, Body b, float time)
{
//Bad code
Vector2 collision = Intersect(a, b);
if (collision.Length() > 2000) return;
//A NORMAL CALCULATION
Vector2 edgea = Vector2.Zero; int cnta = 0; float dista = 10000;
int c = a.shape.Count;
float angle = (float)Math.Acos(-1) / 2;
// DIST MINIMAZING
for (int i = 0; i < c; i++)
{
float d = area(a.shape[i], a.shape[(i + 1) % c], collision - a.position) / (a.shape[i] - a.shape[(i + 1) % c]).Length();
if (d < dista)
dista = d;
}
for (int i = 0; i < c; i++)
{
float d = area(a.shape[i], a.shape[(i + 1) % c], collision - a.position) / (a.shape[i] - a.shape[(i + 1) % c]).Length();
if (Math.Abs(d - dista) < 1e-3 && cnta < 1)
{
Vector2 n = rotate(a.shape[i] - a.shape[(i + 1) % c], angle); n.Normalize();
edgea += n;
cnta++;
}
}
//B NORMAL CALCULATION
Vector2 edgeb = Vector2.Zero; int cntb = 0; float distb = 10000;
c = b.shape.Count;
// DIST MINIMAZING
for (int i = 0; i < c; i++)
{
float d = area(b.shape[i], b.shape[(i + 1) % c], collision - b.position) / (b.shape[i] - b.shape[(i + 1) % c]).Length();
if (d < distb)
distb = d;
}
for (int i = 0; i < c; i++)
{
float d = area(b.shape[i], b.shape[(i + 1) % c], collision - b.position) / (b.shape[i] - b.shape[(i + 1) % c]).Length();
if (Math.Abs(d - distb) < 1e-3 && cntb < 1)
{
Vector2 n = rotate(b.shape[i] - b.shape[(i + 1) % c], angle); n.Normalize();
edgeb += n;
cntb++;
}
}
float cor = a.cor * b.cor / (a.cor + b.cor);
float forcea = cor * (distb + dista) / 100;
float forceb = cor * (dista + distb) / 100;
//Some magic & phusic
if (Math.Abs(dista) < 1e-4) edgea = -edgeb;
if (Math.Abs(distb) < 1e-4) edgeb = -edgea;
Vector2 va = a.velocity; va.Normalize();
Vector2 vb = b.velocity; vb.Normalize();
if (p1(va, edgea) >= 0)
forcea *= a.bounce;
if (p1(vb, edgeb) >= 0)
forceb *= b.bounce;
a.ApplyImpulse(collision - a.position, edgea * forcea / cnta);
b.ApplyImpulse(collision - b.position, edgeb * forceb / cntb);
return;
}
bool BroadPhase(Body a, Body b)
{
Vector2 maxa, mina, maxb, minb;
maxa = mina = a.shape[0] + a.position;
maxb = minb = b.shape[0] + b.position;
#region Cycles
foreach (Vector2 v in a.shape)
{
mina.X = Math.Min(mina.X, v.X + a.position.X);
mina.Y = Math.Min(mina.Y, v.Y + a.position.Y);
maxa.X = Math.Max(maxa.X, v.X + a.position.X);
maxa.Y = Math.Max(maxa.Y, v.Y + a.position.Y);
}
foreach (Vector2 v in b.shape)
{
minb.X = Math.Min(minb.X, v.X + b.position.X);
minb.Y = Math.Min(minb.Y, v.Y + b.position.Y);
maxb.X = Math.Max(maxb.X, v.X + b.position.X);
maxb.Y = Math.Max(maxb.Y, v.Y + b.position.Y);
}
#endregion
if (maxa.X >= minb.X && mina.X <= maxb.X && maxa.Y >= minb.Y && mina.Y <= maxb.Y)
return true;
if (maxb.X >= mina.X && minb.X <= maxa.X && maxb.Y >= mina.Y && minb.Y <= maxa.Y)
return true;
return false;
}
