public static bool TestNarrowPolyPoly(IPolyBody targetP, IPolyBody impactP, out Contact contact) { contact.TargetBody = targetP; contact.ImpactBody = impactP; contact.Disp = 0; contact.Normal = Vector2.Zero; contact.ConPoint = Vector2.Zero; throw new NotImplementedException(); }
public static bool TestNarrowCircleCircle(ICircleBody targetC, ICircleBody impactC, out Contact contact) { contact.TargetBody = targetC; contact.ImpactBody = impactC; contact.Disp = 0; contact.Normal = Vector2.Zero; contact.ConPoint = Vector2.Zero; float disp = (impactC.Position - targetC.Position).Length() - targetC.Radius - impactC.Radius; if (disp <= 0) { var norm = Vector2.Normalize(impactC.Position - targetC.Position); var conpt = targetC.Position + norm * (targetC.Radius + disp / 2); //go out to edge, then in halfway (disp is negative) contact.Disp = disp; contact.Normal = norm; contact.ConPoint = conpt; return true; } else return false; }
/// <summary> /// /// </summary> /// <param name="targetB"></param> /// <param name="impactB"></param> /// <param name="contact"></param> /// <returns></returns> public static bool TestNarrowCollision(IRigidBody targetB, IRigidBody impactB, out Contact contact) { contact.TargetBody = targetB; contact.ImpactBody = impactB; contact.Disp = 0; //default vals so compiler doesn't complain contact.Normal = Vector2.Zero; contact.ConPoint = Vector2.Zero; if (targetB is ICircleBody && impactB is ICircleBody) { return TestNarrowCircleCircle((ICircleBody)targetB, (ICircleBody)impactB, out contact); } else if (targetB is ICircleBody && impactB is IPolyBody) { return TestNarrowCirclePoly((ICircleBody)targetB, (IPolyBody)impactB, out contact); } else if (targetB is IPolyBody && impactB is ICircleBody) { var b = TestNarrowCirclePoly((ICircleBody)impactB, (IPolyBody)targetB, out contact); //invert all contact attributes, since we passed inverted targetB/impactB pair var tmp = contact.TargetBody; contact.TargetBody = contact.ImpactBody; contact.ImpactBody = tmp; contact.Disp *= -1; contact.Normal *= -1; return b; } else if (targetB is IPolyBody && impactB is IPolyBody) { return TestNarrowPolyPoly((IPolyBody)targetB, (IPolyBody)impactB, out contact); } //EVERYTHING BELOW THIS POINT IS HAX UNTIL WE HAVE POLY else if (targetB is ICircleBody && impactB is IVertWallBody) { var targetC = (ICircleBody)targetB; var impactW = (IVertWallBody)impactB; var BB = impactW.BoundingBox; if (targetC.Position.Y + targetC.Radius > BB.YMin && targetC.Position.Y - targetC.Radius < BB.YMax) { if (targetC.Position.X - targetC.Radius < BB.XMax && targetC.Position.X - targetC.Radius > BB.XMin) //right hand wall { contact.Disp = -(BB.XMax - (targetC.Position.X - targetC.Radius)); contact.Normal = -Vector2.UnitX; //from circle to wall return true; } else if (targetC.Position.X + targetC.Radius > BB.XMin && targetC.Position.X + targetC.Radius < BB.XMax) //left hand wall { contact.Disp = -((targetC.Position.X + targetC.Radius) - BB.XMin); contact.Normal = Vector2.UnitX; return true; } contact.ConPoint = targetC.Position + contact.Normal * (targetC.Radius + contact.Disp / 2); } return false; } else if (targetB is ICircleBody && impactB is IHorizWallBody) { var targetC = (ICircleBody)targetB; var impactW = (IHorizWallBody)impactB; var BB = impactW.BoundingBox; if (targetC.Position.X + targetC.Radius > BB.XMin && targetC.Position.X - targetC.Radius < BB.XMax) { if (targetC.Position.Y - targetC.Radius < BB.YMax && targetC.Position.Y - targetC.Radius > BB.YMin) //bottom wall { contact.Disp = -(BB.YMax - (targetC.Position.Y - targetC.Radius)); contact.Normal = -Vector2.UnitY; //from circle to wall return true; } else if (targetC.Position.Y + targetC.Radius > BB.YMin && targetC.Position.Y + targetC.Radius < BB.YMax) //top wall { contact.Disp = -((targetC.Position.Y + targetC.Radius) - BB.YMin); contact.Normal = Vector2.UnitY; return true; } contact.ConPoint = targetC.Position + contact.Normal * (targetC.Radius + contact.Disp / 2); } return false; } else { throw new NotImplementedException(); //SAT //sub-contact detection //...etc. } }
public static CollisionData CalcCollision(Contact con) { var cdata = new CollisionData(); cdata.TargetBody = con.TargetBody; cdata.dPos = Vector2.Zero; cdata.dVel = Vector2.Zero; cdata.dAngVel = 0; var A = con.TargetBody; var B = con.ImpactBody; //penetration resolution var totalInvMass = A.InvMass() + B.InvMass(); cdata.dPos = (con.Disp * A.InvMass() / (totalInvMass)) * con.Normal; //more massive-> less weight. objects in collision share burden of displacing proportional to 1/M. //collision response var sepVel = Vector2.Dot(B.Velocity - A.Velocity, con.Normal); //separating velocity: relative velocity of impact body along normal. if (sepVel >= 0) return cdata; //return no change if bodies are separating float rest = (A.Restitution+B.Restitution)/2; float impulse = sepVel * (1 + rest) / (totalInvMass); cdata.dVel = (impulse * A.InvMass()) * con.Normal; return cdata; }