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; }