public static List <CollisionInfo> Intersects(Body body_a, Body body_b)
{
List <CollisionInfo> data = new List <CollisionInfo>();
int bApmCount = body_a.count;
int bBpmCount = body_b.count;
BoundingSquare boxB = body_b.aabb;
// check all PointMasses on bodyA for collision against bodyB. if there is a collision, return detailed info.
CollisionInfo infoAway = new CollisionInfo();
CollisionInfo infoSame = new CollisionInfo();
for (int i = 0; i < bApmCount; i++)
{
Vector2 pt = body_a.pointmass_list[i].position;
// early out - if this point is outside the bounding box for bodyB, skip it!
if (!boxB.Contains(pt.X, pt.Y))
{
continue;
}
// early out - if this point is not inside bodyB, skip it!
if (!body_b.Contains(ref pt))
{
continue;
}
int prevPt = (i > 0) ? i - 1 : bApmCount - 1;
int nextPt = (i < bApmCount - 1) ? i + 1 : 0;
Vector2 prev = body_a.pointmass_list[prevPt].position;
Vector2 next = body_a.pointmass_list[nextPt].position;
// now get the normal for this point. (NOT A UNIT VECTOR)
Vector2 fromPrev = new Vector2();
fromPrev.X = pt.X - prev.X;
fromPrev.Y = pt.Y - prev.Y;
Vector2 toNext = new Vector2();
toNext.X = next.X - pt.X;
toNext.Y = next.Y - pt.Y;
Vector2 ptNorm = new Vector2();
ptNorm.X = fromPrev.X + toNext.X;
ptNorm.Y = fromPrev.Y + toNext.Y;
VectorHelper.Perpendicular(ref ptNorm);
// this point is inside the other body. now check if the edges on either side intersect with and edges on bodyB.
float closestAway = 100000.0f;
float closestSame = 100000.0f;
infoAway.Clear();
infoAway.body_a = body_a;
infoAway.pointmass_a = body_a.pointmass_list[i];
infoAway.body_b = body_b;
infoSame.Clear();
infoSame.body_a = body_a;
infoSame.pointmass_a = body_a.pointmass_list[i];
infoSame.body_b = body_b;
bool found = false;
int b1 = 0;
int b2 = 1;
for (int j = 0; j < bBpmCount; j++)
{
Vector2 hitPt;
Vector2 norm;
float edgeD;
b1 = j;
if (j < bBpmCount - 1)
{
b2 = j + 1;
}
else
{
b2 = 0;
}
Vector2 pt1 = body_b.pointmass_list[b1].position;
Vector2 pt2 = body_b.pointmass_list[b2].position;
// quick test of distance to each point on the edge, if both are greater than current mins, we can skip!
float distToA = ((pt1.X - pt.X) * (pt1.X - pt.X)) + ((pt1.Y - pt.Y) * (pt1.Y - pt.Y));
float distToB = ((pt2.X - pt.X) * (pt2.X - pt.X)) + ((pt2.Y - pt.Y) * (pt2.Y - pt.Y));
if ((distToA > closestAway) && (distToA > closestSame) && (distToB > closestAway) && (distToB > closestSame))
{
continue;
}
// test against this edge.
float dist = body_b.GetClosestPointOnEdgeSquared(pt, j, out hitPt, out norm, out edgeD);
// only perform the check if the normal for this edge is facing AWAY from the point normal.
float dot;
Vector2.Dot(ref ptNorm, ref norm, out dot);
if (dot <= 0f)
{
if (dist < closestAway)
{
closestAway = dist;
infoAway.pointmass_b = body_b.pointmass_list[b1];
infoAway.pointmass_c = body_b.pointmass_list[b2];
infoAway.edge_distance = edgeD;
infoAway.point = hitPt;
infoAway.normal = norm;
infoAway.penetration = dist;
found = true;
}
}
else
{
if (dist < closestSame)
{
closestSame = dist;
infoSame.pointmass_b = body_b.pointmass_list[b1];
infoSame.pointmass_c = body_b.pointmass_list[b2];
infoSame.edge_distance = edgeD;
infoSame.point = hitPt;
infoSame.normal = norm;
infoSame.penetration = dist;
}
}
}
// we've checked all edges on BodyB.
if ((found) && (closestAway > 0.3f) && (closestSame < closestAway))
{
infoSame.penetration = (float)Math.Sqrt(infoSame.penetration);
data.Add(infoSame);
}
else
{
infoAway.penetration = (float)Math.Sqrt(infoAway.penetration);
data.Add(infoAway);
}
}
return(data);
}
public void Update(double elapsed)
{
if (!initialized)
{
Initialize();
}
penetration_count = 0;
collision_list.Clear();
for (int i = 0; i < body_list.Count; i++)
{
body_list[i].Update(elapsed);
UpdateBitmask(body_list[i]);
}
// update chains
for (int i = 0; i < chain_list.Count; i++)
{
chain_list[i].Update(elapsed);
}
// now check for collision.
// inter-body collision!
for (int i = 0; i < body_list.Count; i++)
{
for (int j = i + 1; j < body_list.Count; j++)
{
if (body_list[i].is_static && body_list[j].is_static)
{
continue;
}
// grid-based early out.
if (((body_list[i].bitmaskx.mask & body_list[j].bitmaskx.mask) == 0) &&
((body_list[i].bitmasky.mask & body_list[j].bitmasky.mask) == 0))
{
continue;
}
// broad-phase collision via AABB.
if (!(body_list[i].aabb).Intersects(ref (body_list[j].aabb)))
{
continue;
}
if (on_aabb_collision != null)
{
this.on_aabb_collision(body_list[i], body_list[j]);
}
// okay, the AABB's of these 2 are intersecting. now check for collision of A against B.
collision_list.AddRange(Collision.Intersects(body_list[j], body_list[i]));
collision_list.AddRange(Collision.Intersects(body_list[i], body_list[j]));
}
}
// now handle all collisions found during the update at once.
// handle all collisions!
for (int i = 0; i < collision_list.Count; i++)
{
CollisionInfo info = collision_list[i];
PointMass A = info.pointmass_a;
PointMass B1 = info.pointmass_b;
PointMass B2 = info.pointmass_c;
if (on_collision != null)
{
this.on_collision(info.body_a, info.body_b, info);
}
// velocity changes as a result of collision.
Vector2 bVel = new Vector2();
bVel.X = (B1.velocity.X + B2.velocity.X) * 0.5f;
bVel.Y = (B1.velocity.Y + B2.velocity.Y) * 0.5f;
Vector2 relVel = new Vector2();
relVel.X = A.velocity.X - bVel.X;
relVel.Y = A.velocity.Y - bVel.Y;
float relDot;
Vector2.Dot(ref relVel, ref info.normal, out relDot);
if (on_penetration != null)
{
if (info.penetration > penetration_threshold)
{
this.on_penetration(info.body_a, info.body_b);
}
}
if (info.penetration > 0.3f)
{
penetration_count++;
continue;
}
float b1inf = 1.0f - info.edge_distance;
float b2inf = info.edge_distance;
float b2MassSum = ((float.IsPositiveInfinity(B1.mass)) || (float.IsPositiveInfinity(B2.mass))) ? float.PositiveInfinity : (B1.mass + B2.mass);
float massSum = A.mass + b2MassSum;
float Amove;
float Bmove;
if (float.IsPositiveInfinity(A.mass))
{
Amove = 0f;
Bmove = (info.penetration) + 0.001f;
}
else if (float.IsPositiveInfinity(b2MassSum))
{
Amove = (info.penetration) + 0.001f;
Bmove = 0f;
}
else
{
Amove = (info.penetration * (b2MassSum / massSum));
Bmove = (info.penetration * (A.mass / massSum));
}
float B1move = Bmove * b1inf;
float B2move = Bmove * b2inf;
float AinvMass = (float.IsPositiveInfinity(A.mass)) ? 0f : 1f / A.mass;
float BinvMass = (float.IsPositiveInfinity(b2MassSum)) ? 0f : 1f / b2MassSum;
float jDenom = AinvMass + BinvMass;
Vector2 numV = new Vector2();
float elas = elasticity;
numV.X = relVel.X * elas;
numV.Y = relVel.Y * elas;
float jNumerator;
Vector2.Dot(ref numV, ref info.normal, out jNumerator);
jNumerator = -jNumerator;
float j = jNumerator / jDenom;
if (!float.IsPositiveInfinity(A.mass))
{
A.position.X += info.normal.X * Amove;
A.position.Y += info.normal.Y * Amove;
}
if (!float.IsPositiveInfinity(B1.mass))
{
B1.position.X -= info.normal.X * B1move;
B1.position.Y -= info.normal.Y * B1move;
}
if (!float.IsPositiveInfinity(B2.mass))
{
B2.position.X -= info.normal.X * B2move;
B2.position.Y -= info.normal.Y * B2move;
}
Vector2 tangent = new Vector2();
VectorHelper.Perpendicular(ref info.normal, ref tangent);
float fNumerator;
Vector2.Dot(ref relVel, ref tangent, out fNumerator);
fNumerator *= friction;
float f = fNumerator / jDenom;
// adjust velocity if relative velocity is moving toward each other.
if (relDot <= 0.0001f)
{
if (!float.IsPositiveInfinity(A.mass))
{
A.velocity.X += (info.normal.X * (j / A.mass)) - (tangent.X * (f / A.mass));
A.velocity.Y += (info.normal.Y * (j / A.mass)) - (tangent.Y * (f / A.mass));
}
if (!float.IsPositiveInfinity(b2MassSum))
{
B1.velocity.X -= (info.normal.X * (j / b2MassSum) * b1inf) - (tangent.X * (f / b2MassSum) * b1inf);
B1.velocity.Y -= (info.normal.Y * (j / b2MassSum) * b1inf) - (tangent.Y * (f / b2MassSum) * b1inf);
}
if (!float.IsPositiveInfinity(b2MassSum))
{
B2.velocity.X -= (info.normal.X * (j / b2MassSum) * b2inf) - (tangent.X * (f / b2MassSum) * b2inf);
B2.velocity.Y -= (info.normal.Y * (j / b2MassSum) * b2inf) - (tangent.Y * (f / b2MassSum) * b2inf);
}
//info.body_a.UpdateBodyPositionVelocityForce(elapsed);
// info.body_b.UpdateBodyPositionVelocityForce(elapsed);
}
}
for (int i = 0; i < body_list.Count; i++)
{
body_list[i].UpdateBodyPositionVelocityForce(elapsed);
}
}
private void collision(Body body_a, Body body_b, CollisionInfo info)
{
if (player.hurt != 0)
return;
// early out, cant be a bubble merge
if (body_a.is_static || body_b.is_static)
return;
if (body_a == player.body)
if (FindEnemy(body_b) != null)
{
player.Hurt();
Resources.droplet_sound.Play(0.025f, ((float)rand.NextDouble() - 0.5f), 0);
}
if (body_b == player.body)
if (FindEnemy(body_a) != null)
{
player.Hurt();
Resources.droplet_sound.Play(0.025f, ((float)rand.NextDouble() - 0.5f), 0);
}
}
public static List<CollisionInfo> Intersects(Body body_a, Body body_b)
{
List<CollisionInfo> data = new List<CollisionInfo>();
int bApmCount = body_a.count;
int bBpmCount = body_b.count;
BoundingSquare boxB = body_b.aabb;
// check all PointMasses on bodyA for collision against bodyB. if there is a collision, return detailed info.
CollisionInfo infoAway = new CollisionInfo();
CollisionInfo infoSame = new CollisionInfo();
for (int i = 0; i < bApmCount; i++)
{
Vector2 pt = body_a.pointmass_list[i].position;
// early out - if this point is outside the bounding box for bodyB, skip it!
if (!boxB.Contains(pt.X, pt.Y))
continue;
// early out - if this point is not inside bodyB, skip it!
if (!body_b.Contains(ref pt))
continue;
int prevPt = (i > 0) ? i - 1 : bApmCount - 1;
int nextPt = (i < bApmCount - 1) ? i + 1 : 0;
Vector2 prev = body_a.pointmass_list[prevPt].position;
Vector2 next = body_a.pointmass_list[nextPt].position;
// now get the normal for this point. (NOT A UNIT VECTOR)
Vector2 fromPrev = new Vector2();
fromPrev.X = pt.X - prev.X;
fromPrev.Y = pt.Y - prev.Y;
Vector2 toNext = new Vector2();
toNext.X = next.X - pt.X;
toNext.Y = next.Y - pt.Y;
Vector2 ptNorm = new Vector2();
ptNorm.X = fromPrev.X + toNext.X;
ptNorm.Y = fromPrev.Y + toNext.Y;
VectorHelper.Perpendicular(ref ptNorm);
// this point is inside the other body. now check if the edges on either side intersect with and edges on bodyB.
float closestAway = 100000.0f;
float closestSame = 100000.0f;
infoAway.Clear();
infoAway.body_a = body_a;
infoAway.pointmass_a = body_a.pointmass_list[i];
infoAway.body_b = body_b;
infoSame.Clear();
infoSame.body_a = body_a;
infoSame.pointmass_a = body_a.pointmass_list[i];
infoSame.body_b = body_b;
bool found = false;
int b1 = 0;
int b2 = 1;
for (int j = 0; j < bBpmCount; j++)
{
Vector2 hitPt;
Vector2 norm;
float edgeD;
b1 = j;
if (j < bBpmCount - 1)
b2 = j + 1;
else
b2 = 0;
Vector2 pt1 = body_b.pointmass_list[b1].position;
Vector2 pt2 = body_b.pointmass_list[b2].position;
// quick test of distance to each point on the edge, if both are greater than current mins, we can skip!
float distToA = ((pt1.X - pt.X) * (pt1.X - pt.X)) + ((pt1.Y - pt.Y) * (pt1.Y - pt.Y));
float distToB = ((pt2.X - pt.X) * (pt2.X - pt.X)) + ((pt2.Y - pt.Y) * (pt2.Y - pt.Y));
if ((distToA > closestAway) && (distToA > closestSame) && (distToB > closestAway) && (distToB > closestSame))
continue;
// test against this edge.
float dist = body_b.GetClosestPointOnEdgeSquared(pt, j, out hitPt, out norm, out edgeD);
// only perform the check if the normal for this edge is facing AWAY from the point normal.
float dot;
Vector2.Dot(ref ptNorm, ref norm, out dot);
if (dot <= 0f)
{
if (dist < closestAway)
{
closestAway = dist;
infoAway.pointmass_b = body_b.pointmass_list[b1];
infoAway.pointmass_c = body_b.pointmass_list[b2];
infoAway.edge_distance = edgeD;
infoAway.point = hitPt;
infoAway.normal = norm;
infoAway.penetration = dist;
found = true;
}
}
else
{
if (dist < closestSame)
{
closestSame = dist;
infoSame.pointmass_b = body_b.pointmass_list[b1];
infoSame.pointmass_c = body_b.pointmass_list[b2];
infoSame.edge_distance = edgeD;
infoSame.point = hitPt;
infoSame.normal = norm;
infoSame.penetration = dist;
}
}
}
// we've checked all edges on BodyB.
if ((found) && (closestAway > 0.3f) && (closestSame < closestAway))
{
infoSame.penetration = (float)Math.Sqrt(infoSame.penetration);
data.Add(infoSame);
}
else
{
infoAway.penetration = (float)Math.Sqrt(infoAway.penetration);
data.Add(infoAway);
}
}
return data;
}