public Fix64Vector2 Support(Fix64Vector2 d) { Fix64 maxDot = Fix64Vector2.Dot(vertex[0], d); Fix64Vector2 point = vertex[0]; for (int i = 1; i < vertex.Length; i++) { Fix64 now = Fix64Vector2.Dot(vertex[i], d); if (now > maxDot) { maxDot = now; point = vertex[i]; } } return(point + GetPos()); }
private static bool ContainsOrigin(Simplex s, ref Fix64Vector2 dir) { var a = s.GetLast(); var AO = -a; var b = s.GetB(); var AB = b - a; if (s.Count() == 3) { var c = s.GetC(); var AC = c - a; var abPerp = CalcNomal(AC, AB, AB); var acPerp = CalcNomal(AB, AC, AC); if (Fix64Vector2.Dot(abPerp, AO) > Fix64.Zero) { s.Remove(c); dir = abPerp; } else { if (Fix64Vector2.Dot(acPerp, AO) > Fix64.Zero) { s.Remove(b); dir = acPerp; } else { return(true); } } } else { var abPerp = CalcNomal(AB, AO, AB); dir = abPerp; } return(false); }
public static bool GJK(MGFObject a, MGFObject b) { Simplex s = new Simplex(); Fix64Vector2 dir = a.GetPos() - b.GetPos(); s.Add(Support(a, b, dir)); dir = -dir; while (true) { s.Add(Support(a, b, dir)); if (Fix64Vector2.Dot(s.GetLast(), dir) <= Fix64.Zero) { return(false); } else { if (ContainsOrigin(s, ref dir)) { return(true); } } } }
internal void CalcCollisionDir(MGFObject mgfObject) { if (IsTrigger == true) { CollisionDir = Forward; return; } Fix64Vector2[] vertexO = mgfObject.GetVertex(); Fix64Vector2 v1 = Fix64Vector2.Rotate(vertexO[0], -(Fix64)90); Fix64Vector2 v2 = Fix64Vector2.Rotate(vertexO[1], -(Fix64)90); Fix64Vector2 v3 = -v1; Fix64Vector2 v4 = -v2; Fix64Vector2 dir = GetPos() - mgfObject.GetPos(); Fix64 d1 = Fix64Vector2.Dot(v1, dir); Fix64 d2 = Fix64Vector2.Dot(v2, dir); Fix64 d3 = Fix64Vector2.Dot(v3, dir); Fix64 d4 = Fix64Vector2.Dot(v4, dir); Fix64Vector2 collisionDirVertical = vertexO[0] - vertexO[1]; Fix64Vector2 collisionDirHorizontal = vertexO[1] - vertexO[2]; if (d1 >= Fix64.Zero && d4 >= Fix64.Zero) { if (Fix64Vector2.Dot(collisionDirHorizontal, Forward) > Fix64.Zero) { CollisionDir = Forward; } else { if (Fix64Vector2.Dot(collisionDirVertical, Forward) > Fix64.Zero) { CollisionDir = collisionDirVertical; } else { CollisionDir = -collisionDirVertical; } } } else if (d1 >= Fix64.Zero && d2 >= Fix64.Zero) { if (Fix64Vector2.Dot(collisionDirVertical, Forward) < Fix64.Zero) { CollisionDir = Forward; } else { if (Fix64Vector2.Dot(collisionDirHorizontal, Forward) > Fix64.Zero) { CollisionDir = collisionDirHorizontal; } else { CollisionDir = -collisionDirHorizontal; } } } else if (d2 >= Fix64.Zero && d3 >= Fix64.Zero) { if (Fix64Vector2.Dot(collisionDirHorizontal, Forward) < Fix64.Zero) { CollisionDir = Forward; } else { if (Fix64Vector2.Dot(collisionDirVertical, Forward) > Fix64.Zero) { CollisionDir = collisionDirVertical; } else { CollisionDir = -collisionDirVertical; } } } else if (d3 >= Fix64.Zero && d4 >= Fix64.Zero) { if (Fix64Vector2.Dot(collisionDirVertical, Forward) > Fix64.Zero) { CollisionDir = Forward; } else { if (Fix64Vector2.Dot(collisionDirHorizontal, Forward) > Fix64.Zero) { CollisionDir = collisionDirHorizontal; } else { CollisionDir = -collisionDirHorizontal; } } } }