public override void ApplyInternalForces(double elapsed)
{
base.ApplyInternalForces(elapsed);
// internal forces based on pressure equations. we need 2 loops to do this. one to find the overall volume of the
// body, and 1 to apply forces. we will need the normals for the edges in both loops, so we will cache them and remember them.
volume = 0f;
for (int i = 0; i < count; i++)
{
int prev = (i > 0) ? i - 1 : count - 1;
int next = (i < count - 1) ? i + 1 : 0;
// currently we are talking about the edge from i --> j.
// first calculate the volume of the body, and cache normals as we go.
Vector2 edge1N = new Vector2();
edge1N.X = pointmass_list[i].position.X - pointmass_list[prev].position.X;
edge1N.Y = pointmass_list[i].position.Y - pointmass_list[prev].position.Y;
VectorHelper.Perpendicular(ref edge1N);
Vector2 edge2N = new Vector2();
edge2N.X = pointmass_list[next].position.X - pointmass_list[i].position.X;
edge2N.Y = pointmass_list[next].position.Y - pointmass_list[i].position.Y;
VectorHelper.Perpendicular(ref edge2N);
Vector2 norm = new Vector2();
norm.X = edge1N.X + edge2N.X;
norm.Y = edge1N.Y + edge2N.Y;
float nL = (float)Math.Sqrt((norm.X * norm.X) + (norm.Y * norm.Y));
if (nL > 0.001f)
{
norm.X /= nL;
norm.Y /= nL;
}
float edgeL = (float)Math.Sqrt((edge2N.X * edge2N.X) + (edge2N.Y * edge2N.Y));
// cache normal and edge length
normal_list[i] = norm;
edgelength_list[i] = edgeL;
float xdist = Math.Abs(pointmass_list[i].position.X - pointmass_list[next].position.X);
float volumeProduct = xdist * Math.Abs(norm.X) * edgeL;
// add to volume
volume += 0.5f * volumeProduct;
}
// now loop through, adding forces!
float invVolume = 1f / volume;
for (int i = 0; i < count; i++)
{
int j = (i < count - 1) ? i + 1 : 0;
float pressureV = (invVolume * edgelength_list[i] * pressure);
pointmass_list[i].force.X += normal_list[i].X * pressureV;
pointmass_list[i].force.Y += normal_list[i].Y * pressureV;
pointmass_list[j].force.X += normal_list[j].X * pressureV;
pointmass_list[j].force.Y += normal_list[j].Y * pressureV;
}
}
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);
}
private void RotateShape(double elapsed)
{
// find the average angle of all of the masses.
float angle = 0;
int originalSign = 1;
float originalAngle = 0;
for (int i = 0; i < count; i++)
{
Vector2 baseNorm = new Vector2();
baseNorm.X = base_shape.points[i].X;
baseNorm.Y = base_shape.points[i].Y;
Vector2.Normalize(ref baseNorm, out baseNorm);
Vector2 curNorm = new Vector2();
curNorm.X = pointmass_list[i].position.X - position.X;
curNorm.Y = pointmass_list[i].position.Y - position.Y;
Vector2.Normalize(ref curNorm, out curNorm);
float dot;
Vector2.Dot(ref baseNorm, ref curNorm, out dot);
if (dot > 1.0f)
{
dot = 1.0f;
}
if (dot < -1.0f)
{
dot = -1.0f;
}
float thisAngle = (float)Math.Acos(dot);
if (!VectorHelper.IsCCW(ref baseNorm, ref curNorm))
{
thisAngle = -thisAngle;
}
if (i == 0)
{
originalSign = (thisAngle >= 0.0f) ? 1 : -1;
originalAngle = thisAngle;
}
else
{
float diff = (thisAngle - originalAngle);
int thisSign = (thisAngle >= 0.0f) ? 1 : -1;
if ((Math.Abs(diff) > Math.PI) && (thisSign != originalSign))
{
thisAngle = (thisSign == -1) ? ((float)Math.PI + ((float)Math.PI + thisAngle)) : (((float)Math.PI - thisAngle) - (float)Math.PI);
}
}
angle += thisAngle;
}
angle = angle / count;
// now calculate the derived Omega, based on change in angle over time.
float angleChange = (angle - prev_angle);
if (Math.Abs(angleChange) >= Math.PI)
{
if (angleChange < 0f)
{
angleChange = angleChange + (float)(Math.PI * 2);
}
else
{
angleChange = angleChange - (float)(Math.PI * 2);
}
}
omega = angleChange / (float)elapsed;
prev_angle = angle;
for (int i = 0; i < count; i++)
{
float x = base_shape.points[i].X * scale.X;
float y = base_shape.points[i].Y * scale.Y;
float c = (float)Math.Cos(angle);
float s = (float)Math.Sin(angle);
curr_shape.points[i].X = (c * x) - (s * y) + position.X;
curr_shape.points[i].Y = (c * y) + (s * x) + position.Y;
}
}
public float GetClosestPointOnEdgeSquared(Vector2 point, int edgeNum, out Vector2 hitPt, out Vector2 normal, out float edgeD)
{
hitPt = new Vector2();
hitPt.X = 0f;
hitPt.Y = 0f;
normal = new Vector2();
normal.X = 0f;
normal.Y = 0f;
edgeD = 0f;
float dist = 0f;
Vector2 ptA = pointmass_list[edgeNum].position;
Vector2 ptB = new Vector2();
if (edgeNum < (count - 1))
{
ptB = pointmass_list[edgeNum + 1].position;
}
else
{
ptB = pointmass_list[0].position;
}
Vector2 toP = new Vector2();
toP.X = point.X - ptA.X;
toP.Y = point.Y - ptA.Y;
Vector2 E = new Vector2();
E.X = ptB.X - ptA.X;
E.Y = ptB.Y - ptA.Y;
// get the length of the edge, and use that to normalize the vector.
float edgeLength = (float)Math.Sqrt((E.X * E.X) + (E.Y * E.Y));
if (edgeLength > 0.00001f)
{
E.X /= edgeLength;
E.Y /= edgeLength;
}
// normal
Vector2 n = new Vector2();
VectorHelper.Perpendicular(ref E, ref n);
// calculate the distance!
float x;
Vector2.Dot(ref toP, ref E, out x);
if (x <= 0.0f)
{
// x is outside the line segment, distance is from pt to ptA.
//dist = (pt - ptA).Length();
Vector2.DistanceSquared(ref point, ref ptA, out dist);
hitPt = ptA;
edgeD = 0f;
normal = n;
}
else if (x >= edgeLength)
{
// x is outside of the line segment, distance is from pt to ptB.
//dist = (pt - ptB).Length();
Vector2.DistanceSquared(ref point, ref ptB, out dist);
hitPt = ptB;
edgeD = 1f;
normal = n;
}
else
{
// point lies somewhere on the line segment.
Vector3 toP3 = new Vector3();
toP3.X = toP.X;
toP3.Y = toP.Y;
Vector3 E3 = new Vector3();
E3.X = E.X;
E3.Y = E.Y;
//dist = Math.Abs(Vector3.Cross(toP3, E3).Z);
Vector3.Cross(ref toP3, ref E3, out E3);
dist = Math.Abs(E3.Z * E3.Z);
hitPt.X = ptA.X + (E.X * x);
hitPt.Y = ptA.Y + (E.Y * x);
edgeD = x / edgeLength;
normal = n;
}
return(dist);
}
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);
}
}