public static void resolveCollision(Rectangle a, VelocityComponent velocityA, Rectangle b, VelocityComponent velocityB, Manifold m, PlayingState playingState, Guid aid, Guid? bid = null)
{
// calculate relative velocity
Vector2 rv = new Vector2(velocityB.xVelocity - velocityA.xVelocity, velocityB.yVelocity - velocityA.yVelocity);
//calculate relative velocity in terms of normal direction
var velAlongNormal = DotProduct(rv, m.normal);
//do not resolve if velocities are seperating
if (velAlongNormal > 0)
{
return;
}
//calculate restitution
var e = Math.Min(restitution, restitution);
//calculate impulse scaler
var j = -(1 + e) * velAlongNormal;
j /= invmass + invmass;
// Apply impulse
Vector2 _impulse = new Vector2(m.normal.X * j, m.normal.Y * j);
velocityA.xVelocity -= (invmass * _impulse.X);
velocityA.yVelocity -= (invmass * _impulse.Y);
velocityB.xVelocity += (invmass * _impulse.X);
velocityB.yVelocity += (invmass * _impulse.Y);
playingState.VelocityComponents[aid] = velocityA;
if (bid != null)
{
playingState.VelocityComponents[(Guid)bid] = velocityB;
}
//float percent = .8f;
//float slop = .01f;
//var c = Math.Max(m.penetration - slop, 0) / (invmass + invmass) * percent * m.normal;
//there is like some position shit that should go here but i dont know what
}
public static Manifold overlapAABB(Rectangle a, VelocityComponent velocityA, Rectangle b, VelocityComponent velocityB)
{
Manifold manifold = new Manifold();
manifold.a = a;
manifold.b = b;
// vectrom from a to b
Vector2 normal = new Vector2(b.X - a.X, b.Y - a.Y);
//calculate half extents along x axis for each object
float a_extent = (a.Width - a.X) / 2;
float b_extent = (b.Width - b.X) / 2;
//calculate overlap on x axis
var x_overlap = a_extent + b_extent - Math.Abs(normal.X);
// SAT test on x axis
if (x_overlap > 0)
{
a_extent = (a.Height - a.X) / 2;
b_extent = (b.Height - b.X) / 2;
//calculate overlap on y axes
var y_overlap = a_extent + b_extent - Math.Abs(normal.Y);
// SAT test on y axis
if (y_overlap > 0)
{
// Find out which axis is axis of least penetration
if (x_overlap < y_overlap)
{
// point towards b knowing that dist points from a to b
if (normal.X < 0)
{
manifold.normal = new Vector2(-1, 0);
}
else
{
manifold.normal = new Vector2(1, 0);
}
manifold.penetration = x_overlap;
return manifold;
}
else
{
// Point toward B knowing that dist points from A to B
if (normal.Y < 0)
{
manifold.normal = new Vector2(0, -1);
}
else
{
manifold.normal = new Vector2(0, 1);
}
manifold.penetration = y_overlap;
return manifold;
}
}
}
return new Manifold();
}