public CollisionResult SolveCollision(Body bodyA, Body bodyB) { ConvexShape polyA = bodyA.Shape as ConvexShape; ConvexShape polyB = bodyB.Shape as ConvexShape; float projectedDistance = 0f; float minPenetration = float.MaxValue; Vector2 distance = bodyA.Position - bodyB.Position; // koska ei oo centterit boxeilla.., turha muuten if (bodyA.Shape as BoxShape != null) { distance = (bodyA.Position + bodyA.Shape.Size) - bodyB.Position; } else if (bodyB.Shape as BoxShape != null) { distance = bodyA.Position - (bodyB.Position + bodyB.Shape.Size); } Vector2 mtv = Vector2.Zero; Vector2[] normals = new Vector2[polyA.Normals.Length + polyB.Normals.Length]; for (int i = 0; i < polyA.Normals.Length; i++) { normals[i] = polyA.Normals[i]; } for (int i = polyA.Normals.Length; i < normals.Length; i++) { normals[i] = polyB.Normals[i - polyA.Normals.Length]; } // joudutaan vetään kaikki checkit koska poly to poly ei oo shortcutteja for (int i = 0; i < normals.Length; i++) { float maxA, minB, maxB; float minA = maxA = minB = maxB = 0f; projectedDistance = Math.Abs(Vector2.Dot(distance, normals[i])); polyA.ProjectOnto(ref normals[i], out minA, out maxA); polyB.ProjectOnto(ref normals[i], out minB, out maxB); float penetration = maxB - minA; // onko seperating axis? if (minA - maxB > 0f || minB - maxA > 0f) { return(CollisionResult.NoCollision); } if (Math.Abs(penetration) < Math.Abs(minPenetration)) { minPenetration = penetration; mtv = normals[i]; } } // jos negatiivinen niin pitää työntää vasemmalle joten flipataan mtv if (Vector2.Dot(distance, mtv) < 0f) { mtv = -mtv; } // törmäys tapahtu return(new CollisionResult() { Us = bodyA, Them = bodyB, Response = minPenetration * mtv, IsColliding = true }); }